From 4f7eca9c39a7f70e72ff9fd57e56e56c817a0ae3 Mon Sep 17 00:00:00 2001 From: "komment-ai[bot]" <122626893+komment-ai[bot]@users.noreply.github.com> Date: Wed, 3 Jul 2024 15:48:37 +0000 Subject: [PATCH 01/10] Added comments to 56 functions across 7 files --- .komment/00000.json | 1909 +++++++++++++++++ .komment/komment.json | 21 + agents/g2o_agent/src/genericworker.py | 36 + agents/g2o_agent/src/specificworker.py | 222 ++ .../scripts/long_term_graph.py | 36 + .../scripts/main.py | 68 + .../src/genericworker.py | 36 + .../src/long_term_graph.py | 41 + .../src/specificworker.py | 284 +++ 9 files changed, 2653 insertions(+) create mode 100644 .komment/00000.json create mode 100644 .komment/komment.json diff --git a/.komment/00000.json b/.komment/00000.json new file mode 100644 index 00000000..2c1dfcb3 --- /dev/null +++ b/.komment/00000.json @@ -0,0 +1,1909 @@ +[ + { + "name": "long_term_graph.py", + "path": "agents/long_term_spatial_memory_agent/src/long_term_graph.py", + "content": { + "structured": { + "description": "A class `RoomFrame` that represents a 3D room frame and its corresponding objects (walls, floors, ceilings, doors, and windows) in a graph structure. The code provides methods for drawing the room frame and its objects on an axis-aligned bounding box (AABB) in 2D or 3D space using Matplotlib. The `get_room_corners` method recursively traverses the graph to compute the projective coordinates of the corners of a room, while the `get_room_objects_coordinates` method computes the coordinates of objects of a specific type within a room. Both methods return lists of 4D vectors representing the position and orientation of each object or corner in the room frame's coordinate system.", + "items": [ + { + "id": "8aa24592-d2b2-699c-8b4a-818d3e97115b", + "ancestors": [], + "description": "Performs a series of actions: it reads and computes a graph from a text file, draws the graph using Matplotlib, and provides methods for navigating and querying the graph.", + "attributes": [ + { + "name": "g", + "type_name": "undetected", + "description": "Used to store the graph data structure for computing the metric reconstruction." + }, + { + "name": "read_graph", + "type_name": "method", + "description": "Responsible for reading a graph from a file in Graphviz format. It takes no arguments and returns a Graph object." + }, + { + "name": "fig", + "type_name": "matplotlibfigureFigure", + "description": "Used to represent the figure object for visualizing the graph." + }, + { + "name": "ax", + "type_name": "PyQt5QtWidgetsQGraphicsScene", + "description": "Used to represent the graphical display area where the graph will be drawn." + }, + { + "name": "fig_2", + "type_name": "matplotlibfigureFigure", + "description": "Used to store the figure object for the graph display." + }, + { + "name": "ax_2", + "type_name": "matplotlibFigure", + "description": "Used to store a second axis object for plotting the graph." + } + ], + "name": "LongTermGraph", + "location": { + "start": 15, + "insert": 16, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "class", + "length": 230, + "docLength": null + }, + { + "id": "82d776a5-ee96-48ae-9e44-d4548fd1800e", + "ancestors": [ + "8aa24592-d2b2-699c-8b4a-818d3e97115b" + ], + "description": "Reads a graph from a file, creates two subplots in an matplotlib figure for visualizing the graph and its metric reconstruction, and sets up labels for the x- and y-axes. If the file is not found, it sets `self.g` to `None`.", + "params": [ + { + "name": "file_name", + "type_name": "str", + "description": "Used to specify the path of a graph file that contains a directed multigraph, which will be read by the function and processed for metric reconstruction." + } + ], + "returns": null, + "name": "__init__", + "location": { + "start": 16, + "insert": 17, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 22, + "docLength": null + }, + { + "id": "61ef881c-4d99-fb93-ec44-afbc9d8a0b4d", + "ancestors": [ + "8aa24592-d2b2-699c-8b4a-818d3e97115b" + ], + "description": "Generates a graphical representation of a subgraph of a larger graph, based on node and edge attributes. It creates a scatter plot of room and door nodes, and plots edges as grey lines with node names annotated.", + "params": [ + { + "name": "only_rooms", + "type_name": "bool", + "description": "Used to filter out nodes that are not rooms, doors, or walls." + } + ], + "returns": null, + "name": "draw_graph", + "location": { + "start": 214, + "insert": 215, + "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 class named \"G2ORealtime\" that implements the G2O real-time algorithm for robot localization and mapping in a graphical representation. The code includes several methods for updating the graph, adding edges, and tracking the position of vertices over time. It also includes functions for updating node attributes and deleting nodes or edges. Overall, the code provides a robust framework for implementing the G2O real-time algorithm in a graphical environment.", + "items": [ + { + "id": "84c303b7-592e-bca1-e44c-174991e2df71", + "ancestors": [], + "description": "Performs real-time visual odometry estimation for a robot using a G2O graph and RT communication. It initializes a graph, adds nominal corners, and updates edges with measured data from an RT communication agent.", + "attributes": [ + { + "name": "Period", + "type_name": "float", + "description": "Used to control the interval at which the worker performs its tasks, with a default value of 1 second. It can be used to adjust the worker's execution speed." + }, + { + "name": "agent_id", + "type_name": "int", + "description": "A unique identifier assigned to each agent in the system." + }, + { + "name": "g", + "type_name": "Graph", + "description": "Used for representing the graph of a robot's motion, where each node represents a pose of the robot and each edge represents a motion of the robot between two poses. The graph is used to compute the marginal probability distribution of the robot's motion given its initial position and velocity." + }, + { + "name": "startup_check", + "type_name": "QTimersingleShot", + "description": "Used to start a quit timer after 200 milliseconds of inactivity, which allows the worker to exit gracefully when no further work is available." + }, + { + "name": "rt_api", + "type_name": "str", + "description": "8 characters long. It stores a string value that represents the ROS launch file path for the RT node." + }, + { + "name": "inner_api", + "type_name": "instance", + "description": "Used as a reference to the inner API object, which is responsible for handling the actual work of the worker." + }, + { + "name": "odometry_node_id", + "type_name": "int", + "description": "0-based index of the node representing the robot's odometry information in the graph. It is used to identify the node in the graph that represents the robot's current position and velocity." + }, + { + "name": "odometry_queue", + "type_name": "3D", + "description": "A queue that stores the odometry data of the robot, which is used to update the graph and calculate the robot's pose." + }, + { + "name": "last_odometry", + "type_name": "3D", + "description": "Used to store the last known odometry value of the robot, which is used for calculating the displacement and covariance matrix." + }, + { + "name": "g2o", + "type_name": "3D", + "description": "A G2O graph, which is a data structure used to represent the robot's pose in a 3D environment. It stores the information of the robot's pose in a set of nodes and edges, where each node represents a location in space and each edge represents a connection between two locations. The `g2o` attribute is used to perform optimization tasks such as finding the robot's pose that minimizes a cost function." + }, + { + "name": "odometry_noise_std_dev", + "type_name": "float", + "description": "Used to represent the standard deviation of odometry noise in the worker's graph. It is used in the `get_displacement` method to compute the displacement between two poses based on the odometry noise." + }, + { + "name": "odometry_noise_angle_std_dev", + "type_name": "floatingpoint", + "description": "1.5 by default, which represents the standard deviation of angle noise in the odometry measurement of a robot. It affects the accuracy of the robot's positioning and movement tracking." + }, + { + "name": "measurement_noise_std_dev", + "type_name": "float", + "description": "10% of the robot's minimum distance from its reference configuration to its target position, indicating the standard deviation of measurement noise in the robot's pose estimation." + }, + { + "name": "last_room_id", + "type_name": "int", + "description": "Used to store the previous room ID that was visited by the agent before the current room. It is updated when the agent moves to a new room, allowing the worker to detect changes in the environment." + }, + { + "name": "actual_room_id", + "type_name": "int", + "description": "Used to store the current room ID of the agent, which is updated when the robot moves from one room to another." + }, + { + "name": "elapsed", + "type_name": "int", + "description": "Used to store the time elapsed since the last call to the `startup_check` function, which is called every 0.1 seconds. It is used to determine when to quit the application." + }, + { + "name": "room_initialized", + "type_name": "bool", + "description": "Used to keep track of whether the current room has been initialized or not. It's set to False when a new room is entered, and True otherwise." + }, + { + "name": "iterations", + "type_name": "int", + "description": "Used to keep track of the number of iterations performed by the worker. It increases by one each time a new iteration is performed and can be accessed or modified from within the worker's methods." + }, + { + "name": "hide", + "type_name": "str", + "description": "Used to specify whether a worker should be hidden or not when multiple workers are running simultaneously." + }, + { + "name": "init_graph", + "type_name": "bool", + "description": "Used to indicate whether the graph has been initialized or not. It is set to True when the graph is initialized and False otherwise, allowing the worker to distinguish between initialized and uninitialized graphs." + }, + { + "name": "current_edge_set", + "type_name": "bool", + "description": "Used to indicate whether the current edge set has been computed or not. It is set to true when the edge set is first computed and false otherwise, so that only new computations are performed." + }, + { + "name": "first_rt_set", + "type_name": "bool", + "description": "Set to `True` when a new RT translation and rotation are detected, and `False` otherwise. It indicates whether the worker has received its first RT set or not." + }, + { + "name": "translation_to_set", + "type_name": "3D", + "description": "Used to store the translation from the robot's reference frame to the set frame." + }, + { + "name": "rotation_to_set", + "type_name": "3D", + "description": "Used to store the rotation of a robot's end effector relative to its base frame, which is necessary for setting the desired orientation of the robot's end effector." + }, + { + "name": "room_polygon", + "type_name": "numpyarray", + "description": "Used to store the coordinates of a room's boundary in a list of (x,y) tuples." + }, + { + "name": "initialize_g2o_graph", + "type_name": "instance", + "description": "Used to initialize a Graph2ONode graph for a specific robotic arm. It creates nodes, edges, and defines the room node, and sets up the optimization algorithm and visualization tools." + }, + { + "name": "rt_set_last_time", + "type_name": "float", + "description": "Used to store the last time when a RT set was set by the worker, which is used to filter out unnecessary RT sets." + }, + { + "name": "rt_time_min", + "type_name": "float", + "description": "Set to the minimum time between two consecutive RT sets. It is used to determine when to reset the translation and rotation values for RT tracking purposes." + }, + { + "name": "timer", + "type_name": "QTimer", + "description": "Used to schedule periodic updates of the graph with the current robot state. It is set to single shot every 200 milliseconds, allowing for real-time updates of the graph." + }, + { + "name": "compute", + "type_name": "instance", + "description": "Used to compute the marginal probability distributions of the robot's state given its current observations. It takes an argument `vertex` which is a vertex in the graph representing the robot's state, and returns a tuple containing the marginal probabilities and the Hessian matrix of the robot's state." + }, + { + "name": "update_node_att", + "type_name": "update", + "description": "Called when a new node with the specified id is added to the graph, or an existing node's attributes are updated. It updates the odometry queue based on the new node's attributes and sets the room initialization flag accordingly." + }, + { + "name": "update_edge", + "type_name": "edge", + "description": "Used to update the attributes of an edge in a graph when the edge's node types change or when a new edge is added to the graph. It sets the `rt_translation` and `rotation_euler_xyz` attributes of the edge based on the new node types." + }, + { + "name": "update_edge_att", + "type_name": "attribute", + "description": "Used to update the attributes of an edge in a graph. It takes the edge ID, the new value for the attribute, and the name of the attribute as input parameters." + } + ], + "name": "SpecificWorker", + "location": { + "start": 50, + "insert": 51, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "class", + "length": 302, + "docLength": null + }, + { + "id": "25b76ae0-c0dd-c6a7-fc4e-5e6acf507c0a", + "ancestors": [ + "84c303b7-592e-bca1-e44c-174991e2df71" + ], + "description": "Initializes a SpecificWorker instance, defining its attributes and connections to graphs, timers, and signal handlers.", + "params": [ + { + "name": "proxy_map", + "type_name": "dict", + "description": "Used to store a mapping from agent IDs to graphs." + }, + { + "name": "startup_check", + "type_name": "Optionalbool", + "description": "Used to check if the robot's graph has already been initialized before starting the main loop." + } + ], + "returns": null, + "name": "__init__", + "location": { + "start": 51, + "insert": 52, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 51, + "docLength": null + }, + { + "id": "1223cd80-b422-d1a2-544e-d32d51f481f3", + "ancestors": [ + "84c303b7-592e-bca1-e44c-174991e2df71" + ], + "description": "Generates high-quality summaries of Java code that is given to it. It takes into account the robot's odometry, computes the translation and rotation of the robot relative to a reference frame, and adds the information to the graph. It also updates the landmark in the graph.", + "params": [], + "returns": null, + "name": "compute", + "location": { + "start": 120, + "insert": 122, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 71, + "docLength": null + }, + { + "id": "3fbfed7d-c225-7ead-9a4a-90ba5ee3ffd1", + "ancestors": [ + "84c303b7-592e-bca1-e44c-174991e2df71" + ], + "description": "Initializes a Graph2O graph for a specific robot and room, adding nominal corners and measuring them to create a polygonal representation of the room. It also sets the robot's initial position and orientation in the graph.", + "params": [], + "returns": { + "type_name": "bool", + "description": "1 if the g2o graph was successfully initialized with the given room id, and 0 otherwise." + }, + "name": "initialize_g2o_graph", + "location": { + "start": 214, + "insert": 215, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 62, + "docLength": null + }, + { + "id": "253cc8ea-c52a-78a0-8844-da420c7a91fe", + "ancestors": [ + "84c303b7-592e-bca1-e44c-174991e2df71" + ], + "description": "Calculates the displacement of a robot based on its odometry data, updating the robot's position and orientation using the lateral, angular, and linear displacements.", + "params": [ + { + "name": "odometry", + "type_name": "stdvectordouble", + "description": "3D vector representing the robot's position, orientation, and velocity at a given time stamp." + } + ], + "returns": { + "type_name": "triplet", + "description": "3 elements long (desplazamiento lateral, displacement avance and angular displacement)." + }, + "name": "get_displacement", + "location": { + "start": 285, + "insert": 286, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 14, + "docLength": null + }, + { + "id": "35fb82ad-3303-53a5-4b40-b15c8f361fa3", + "ancestors": [ + "84c303b7-592e-bca1-e44c-174991e2df71" + ], + "description": "Computes the covariance matrix of a set of vertices in a graph using the G2O optimization algorithm. It takes a vertex as input, returns the computed covariance matrix and a result flag indicating whether the computation was successful.", + "params": [ + { + "name": "vertex", + "type_name": "g2oVertex", + "description": "Passed to compute marginals for computing covariance matrix." + } + ], + "returns": { + "type_name": "tuple", + "description": "2-element, containing the result of computing covariance matrix and the marginals of the vertices." + }, + "name": "get_covariance_matrix", + "location": { + "start": 304, + "insert": 305, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 11, + "docLength": null + }, + { + "id": "16329532-8cfc-7083-4b42-b215340de3c5", + "ancestors": [ + "84c303b7-592e-bca1-e44c-174991e2df71" + ], + "description": "Visualizes a G2O optimization problem in real-time, loading the problem from an file and plotting the vertices and edges as 3D scatter plots while updating the display with small intervals of time.", + "params": [ + { + "name": "optimizer", + "type_name": "instance", + "description": "Used to load G2O files for visualization in real-time." + } + ], + "returns": null, + "name": "visualize_g2o_realtime", + "location": { + "start": 317, + "insert": 318, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 29, + "docLength": null + }, + { + "id": "06023ea6-5dc9-1c87-3541-4399e058c863", + "ancestors": [ + "84c303b7-592e-bca1-e44c-174991e2df71" + ], + "description": "Updates the attributes of a node in a graph, specifically the `odometry_queue` attribute, when the node's ID matches the `odometry_node_id`.", + "params": [ + { + "name": "id", + "type_name": "int", + "description": "Used to identify the node for which attribute values are being updated." + }, + { + "name": "attribute_names", + "type_name": "[str]", + "description": "A list of attribute names to update in the node." + } + ], + "returns": null, + "name": "update_node_att", + "location": { + "start": 354, + "insert": 363, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 7, + "docLength": null + }, + { + "id": "fcfa7684-299e-0cbf-024c-e68fe02c0453", + "ancestors": [ + "84c303b7-592e-bca1-e44c-174991e2df71" + ], + "description": "Updates the node with the specified ID, based on the type of node it represents.", + "params": [ + { + "name": "id", + "type_name": "int", + "description": "Used to identify the node being updated." + }, + { + "name": "type", + "type_name": "str", + "description": "Used to specify the node type." + } + ], + "returns": null, + "name": "update_node", + "location": { + "start": 370, + "insert": 380, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 6, + "docLength": null + }, + { + "id": "fced3417-1e29-05b8-ce46-5f86e6d4eff4", + "ancestors": [ + "84c303b7-592e-bca1-e44c-174991e2df71" + ], + "description": "Deletes a node from the graph represented by the `SpecificWorker` class.", + "params": [ + { + "name": "id", + "type_name": "int", + "description": "Used to identify the node to be deleted." + } + ], + "returns": null, + "name": "delete_node", + "location": { + "start": 382, + "insert": 383, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 6, + "docLength": null + }, + { + "id": "8392cc88-c41a-3d84-7741-474d984fa20d", + "ancestors": [ + "84c303b7-592e-bca1-e44c-174991e2df71" + ], + "description": "Updates the room ID and sets the current edge set based on the provided edge type and node types.", + "params": [ + { + "name": "fr", + "type_name": "int", + "description": "Representative of the start node of an edge in a graph." + }, + { + "name": "to", + "type_name": "int", + "description": "Used to represent the target node of an edge in the graph." + }, + { + "name": "type", + "type_name": "str", + "description": "Used to specify the type of edge to update, either \"current\" or \"RT\"." + } + ], + "returns": null, + "name": "update_edge", + "location": { + "start": 391, + "insert": 392, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 22, + "docLength": null + }, + { + "id": "644bd490-10bb-9e96-d442-ff5ba8edb07d", + "ancestors": [ + "84c303b7-592e-bca1-e44c-174991e2df71" + ], + "description": "Deletes an edge from a graph, specified by the vertex indices `fr` and `to`, and the edge type.", + "params": [ + { + "name": "fr", + "type_name": "int", + "description": "Used to represent the starting vertex index of an edge to be deleted." + }, + { + "name": "to", + "type_name": "int", + "description": "Used to indicate the destination node ID for edge deletion." + }, + { + "name": "type", + "type_name": "str", + "description": "Used to specify the edge type to be deleted, with possible values 'in' or 'out'." + } + ], + "returns": null, + "name": "delete_edge", + "location": { + "start": 419, + "insert": 420, + "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 for killing itself after a specified period. The code also includes Ice loadSlice command to load an Ice slice file, imports RoboCompCommonBehavior module, and sets up a GUI using PySide2.", + "items": [ + { + "id": "19efd488-1ac0-7f99-eb42-0c5514f4d762", + "ancestors": [], + "description": "Provides a mechanism for killing itself and setting a period for a timer. It has a signal `kill` that is emitted when the object is killed, and a method `setPeriod` to set the timer period.", + "attributes": [ + { + "name": "kill", + "type_name": "QtCoreSignal", + "description": "Used to signal the object's termination." + }, + { + "name": "ui", + "type_name": "Ui_guiDlg", + "description": "Used to store the user interface of a GUI dialog box." + }, + { + "name": "mutex", + "type_name": "QMutex", + "description": "Used to protect the internal state of the worker object." + }, + { + "name": "Period", + "type_name": "int", + "description": "500 milliseconds by default, used to set the time interval for the timer event emitted by the `setPeriod()` method." + }, + { + "name": "timer", + "type_name": "QtCoreQTimer", + "description": "Used to start a timer that calls the `setPeriod` slot when it expires." + } + ], + "name": "GenericWorker", + "location": { + "start": 43, + "insert": 45, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "class", + "length": 26, + "docLength": null + }, + { + "id": "47585d96-d782-50be-9d43-a6041210bddd", + "ancestors": [ + "19efd488-1ac0-7f99-eb42-0c5514f4d762" + ], + "description": "Initializes an instance of the `GenericWorker` class by setting up the UI, creating a mutex for thread-safe access to the `Period` variable, and starting a timer with a 500ms delay.", + "params": [ + { + "name": "mprx", + "type_name": "Ui_guiDlg", + "description": "Used to set up the user interface for the GenericWorker class." + } + ], + "returns": null, + "name": "__init__", + "location": { + "start": 47, + "insert": 48, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 9, + "docLength": null + }, + { + "id": "ad8b787d-cd38-c9a5-2048-7db5d71e901b", + "ancestors": [ + "19efd488-1ac0-7f99-eb42-0c5514f4d762" + ], + "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": "f5aaf03a-bb47-4a92-7f4d-501fec240a0e", + "ancestors": [ + "19efd488-1ac0-7f99-eb42-0c5514f4d762" + ], + "description": "Sets the period of a timer and prints a message to the console when it changes.", + "params": [ + { + "name": "p", + "type_name": "int", + "description": "Used to set the new period value for the application's 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": "An autonomous robot navigation agent using Graphical User Interface (GUI) for human interaction and a graph (G) to represent the environment. The code performs various tasks such as:\n\n* Reading from a json file containing the environment data and initializing the GUI window\n* Drawing a room image and displaying it on the GUI\n* Creating edges and nodes in the graph representing the environment, robot, and doors\n* Updating node attributes based on user input\n* Inserting current edge to the room\n* Starting the robot navigation process\n\nThe code uses various packages such as `numpy`, `cv2`, `QTimer`, and `QApplication` for data manipulation, image processing, and GUI functionality.", + "items": [ + { + "id": "76fa4ef8-3f83-f299-c341-308421fee7c7", + "ancestors": [], + "description": "Is responsible for processing specific types of nodes and edges in a graph, such as room nodes and their corresponding doors, as well as affordance nodes and their transitions. It also handles updating the graph's attributes and inserting new edges.", + "attributes": [ + { + "name": "Period", + "type_name": "float32", + "description": "0 by default. It represents the period of time taken for a specific worker to complete its task, which can be used to determine the next task to assign to the worker based on its current state." + }, + { + "name": "agent_id", + "type_name": "int", + "description": "0 by default, representing the agent's internal ID for distinguishing it from other agents." + }, + { + "name": "g", + "type_name": "Graph", + "description": "Used to store the graph data structure for the environment map navigation. It contains the nodes, edges, and other attributes needed for the worker's functionality." + }, + { + "name": "update_node", + "type_name": "attribute", + "description": "Called when a node's attributes change. It updates the state of the worker based on the new attributes." + }, + { + "name": "update_edge", + "type_name": "Edge", + "description": "Used to update the edge attributes based on the robot's movement. It takes three parameters: `fr`, `to`, and `type`, which represent the from, to, and edge type respectively. The method updates the edge attributes accordingly and sets the current edge as \"current\" if there are no existing \"current\" edges." + }, + { + "name": "startup_check", + "type_name": "QTimersingleShot", + "description": "Used to schedule the worker's main loop to run after 200 milliseconds of starting the application." + }, + { + "name": "rt_api", + "type_name": "Attribute", + "description": "Used to store the RT API of the worker, which is needed for communication between the worker and the main agent." + }, + { + "name": "inner_api", + "type_name": "internal", + "description": "Used by the worker to interact with its inner agent API, which handles the actual robot control and sensor data processing." + }, + { + "name": "robot_name", + "type_name": "str", + "description": "Used to store the name of the robot that the worker is controlling." + }, + { + "name": "robot_id", + "type_name": "int", + "description": "Used to store the ID of the robot that the worker represents." + }, + { + "name": "last_robot_pose", + "type_name": "Attribute", + "description": "Used to store the last known pose of the robot in the environment, which can be used for navigation and task completion purposes." + }, + { + "name": "state", + "type_name": "str", + "description": "Used to keep track of the worker's current state, either \"idle\", \"running\", or \"crossed\"." + }, + { + "name": "affordance_node_active_id", + "type_name": "int", + "description": "Used to store the ID of the affordance node that is currently active, indicating which affordance the worker should perform." + }, + { + "name": "exit_door_id", + "type_name": "int", + "description": "0-based, representing the index of the door through which the robot exits a room when it moves from that room to another." + }, + { + "name": "room_exit_door_id", + "type_name": "int", + "description": "14 by default. It represents the ID of the door through which the robot exits a room." + }, + { + "name": "enter_room_node_id", + "type_name": "int", + "description": "Used to store the ID of the room node that the robot enters when it moves from the previous room to the current room." + }, + { + "name": "graph", + "type_name": "Graph", + "description": "Used to store the graph representation of the environment, including nodes, edges, and their attributes." + }, + { + "name": "vertex_size", + "type_name": "int", + "description": "Used to represent the size of a vertex in a graph, indicating its importance or relevance in the graph structure." + }, + { + "name": "not_required_attrs", + "type_name": "list", + "description": "Used to store the names of attributes that are not required for the worker's functionality, i.e., they are optional or not used in the implementation." + }, + { + "name": "long_term_graph", + "type_name": "Graph", + "description": "Used to store the long-term graph of the agent, which is updated at each iteration of the worker. It keeps track of the agent's internal graph over time, allowing the worker to perform actions based on the historical information of the environment." + }, + { + "name": "global_map", + "type_name": "ndarray", + "description": "2D, representing a map of the environment. It stores the occupancy probabilities of each cell in the grid, which are used to calculate the worker's attention and motion." + }, + { + "name": "insert_current_edge", + "type_name": "Edge", + "description": "Used to insert a new edge into the graph with a specified type (current) and source and destination nodes, signaling that the current room has been reached and the agent should move on to the next task." + }, + { + "name": "timer", + "type_name": "QTimer", + "description": "Used to schedule the worker's startup check after a delay of 200 milliseconds." + }, + { + "name": "compute", + "type_name": "instance", + "description": "Used to compute the next action for the agent based on the current state of the environment. It takes into account the agent's attributes, such as its position and orientation, and returns a dict with the next action as key and the corresponding value as value." + } + ], + "name": "SpecificWorker", + "location": { + "start": 49, + "insert": 50, + "offset": " ", + "indent": 4, + "comment": null + }, + "item_type": "class", + "length": 508, + "docLength": null + }, + { + "id": "30924e5c-a8d3-aabb-bf47-aa7f060f80a2", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Sets up the internal structure and event connections for a SpecificWorker instance, including its graph, node ID, and timer for computing updates.", + "params": [ + { + "name": "proxy_map", + "type_name": "igraphGraph", + "description": "Used to specify the graph for this specific worker instance." + }, + { + "name": "startup_check", + "type_name": "bool", + "description": "Used to check if any of the required nodes are present in the graph before starting the agent's inner loop." + } + ], + "returns": null, + "name": "__init__", + "location": { + "start": 50, + "insert": 51, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 45, + "docLength": null + }, + { + "id": "9d1d8a47-e09e-bba1-ea44-c61dcc5ed6f9", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Sets parameters and updates node attributes in a graph. It removes a self-edge from a room, stores an exit door ID, and assigns other_side_door attributes to both doors leading to the new room.", + "params": [ + { + "name": "params", + "type_name": "object", + "description": "Passed to the function for configuration purposes." + } + ], + "returns": { + "type_name": "Boolean", + "description": "True." + }, + "name": "setParams", + "location": { + "start": 115, + "insert": 116, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 7, + "docLength": null + }, + { + "id": "6d2c558e-51df-ebbf-9b45-c373d8e16ac8", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Performs various actions based on the current state of the worker, including idle, crossing, crossed, initializing room, known room, initializing doors, storing graph, and removing.", + "params": [], + "returns": null, + "name": "compute", + "location": { + "start": 129, + "insert": 148, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 24, + "docLength": null + }, + { + "id": "00a4e420-1c1d-efa6-f340-28f5c78e4e52", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Determines if the agent is entering or exiting a room based on the affordance graph and updates the agent's state and door associations accordingly.", + "params": [], + "returns": null, + "name": "idle", + "location": { + "start": 167, + "insert": 169, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 64, + "docLength": null + }, + { + "id": "bd4ee7e6-5efa-6487-9e4f-53d59567192c", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Determines if the agent has reached a door and, if so, updates its state based on the door's properties.", + "params": [], + "returns": null, + "name": "crossed", + "location": { + "start": 258, + "insert": 260, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 16, + "docLength": null + }, + { + "id": "cc00d913-dfa4-edab-9447-2568f6c1e7d6", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Determines and stores the ID of the room node to enter when it is called, and updates the state of the worker to \"initializing doors\".", + "params": [], + "returns": null, + "name": "initializing_room", + "location": { + "start": 278, + "insert": 281, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 10, + "docLength": null + }, + { + "id": "53f4c0b8-56dd-07b3-0440-be3e6683e8df", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Performs a series of actions to handle a robot's movement through a door, including:\n\n1. Finding the other side room node in the graph.\n2. Creating a new edge and vertex for the robot's new location.\n3. Deleting the original edge connecting the robot to the room.", + "params": [], + "returns": null, + "name": "known_room", + "location": { + "start": 292, + "insert": 294, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 56, + "docLength": null + }, + { + "id": "2fab6259-032f-8abf-4744-db2e9c5a2fe7", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "1) retrieves exit edges and matching same-side doors, 2) sets other side door name and connected room name attributes for each exit door node, and 3) associates doors between nodes using their names.", + "params": [], + "returns": null, + "name": "initializing_doors", + "location": { + "start": 362, + "insert": 364, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 28, + "docLength": null + }, + { + "id": "50fd66d9-17e8-659a-6941-9c2265014ec1", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Takes two door nodes as input and connects them by adding an edge between them, while also updating the \"other side door name\" and \"connected room name\" attributes of each node to reflect the connection.", + "params": [ + { + "name": "door_1", + "type_name": "str", + "description": "Represented as an tuple containing the name of the door and its room, such as (\"Door 1\", \"Room 1\")." + }, + { + "name": "door_2", + "type_name": "str", + "description": "Represented as a string containing the name of the second door to be associated with the graph." + } + ], + "returns": null, + "name": "associate_doors", + "location": { + "start": 401, + "insert": 403, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 16, + "docLength": null + }, + { + "id": "a63ad4a9-7162-9788-6f48-a0ea5ee1a361", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Stores the graph represented by an instance of `GenericWorker` in a file named \"graph.pkl\".", + "params": [], + "returns": null, + "name": "store_graph", + "location": { + "start": 420, + "insert": 421, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 11, + "docLength": null + }, + { + "id": "72e8cae2-7f84-98bc-4243-263cb1655204", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Removes edges from the graph that connect two nodes with the same room number, and also deletes nodes that have only one connection to another node with a room number of 200 (shadow nodes). The function then updates the long-term graph and sets the state to \"idle\".", + "params": [], + "returns": null, + "name": "removing", + "location": { + "start": 434, + "insert": 436, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 19, + "docLength": null + }, + { + "id": "64fe063f-2993-c6bc-aa4d-981c1f3a4e37", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Iterates through the robot's graph, identifying and traversing RT edges, and inserting vertices and edges into an IGraph object.", + "params": [ + { + "name": "node_id", + "type_name": "int", + "description": "A unique identifier for the node being traversed." + } + ], + "returns": null, + "name": "traverse_graph", + "location": { + "start": 460, + "insert": 462, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 7, + "docLength": null + }, + { + "id": "426f44c0-7eaf-b78f-2949-a354da7afe47", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Iterates through the successors of a given vertex in a graph, and inserts vertices and edges into a data structure called `dsr` if the successor has a higher level than the current vertex and its name is not already present in `dsr`.", + "params": [ + { + "name": "node", + "type_name": "igraphVertex", + "description": "Represented by an index." + } + ], + "returns": null, + "name": "traverse_igraph", + "location": { + "start": 470, + "insert": 471, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 10, + "docLength": null + }, + { + "id": "6537e0aa-a290-a4aa-e34a-a6fcf04c5f78", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Adds a new vertex to an igraph graph, updating its attributes and linking it with other vertices based on their names.", + "params": [ + { + "name": "node", + "type_name": "igraphNode", + "description": "Passed the vertex to be added to the graph." + } + ], + "returns": null, + "name": "insert_igraph_vertex", + "location": { + "start": 483, + "insert": 484, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 28, + "docLength": null + }, + { + "id": "bf521f4e-8469-df93-ed46-38f37fa5b135", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Creates a new node in the graph and links it to its parent node by setting the `parent` attribute. It also sets other attributes based on the input node data and inserts the new node into the graph using the `insert_node` method.", + "params": [ + { + "name": "parent_name", + "type_name": "str", + "description": "Representative of the name of a node to which a new vertex will be added as its parent." + }, + { + "name": "node", + "type_name": "Node", + "description": "Passed as an instance of the class representing a graph node, containing attributes such as agent ID and type." + } + ], + "returns": null, + "name": "insert_dsr_vertex", + "location": { + "start": 518, + "insert": 520, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 9, + "docLength": null + }, + { + "id": "e2dcf73b-935c-9fb6-d447-fc3b778deaae", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Adds an edge to an igraph graph, specifying the origin and destination nodes, as well as the rotation and translation attributes of the edge.", + "params": [ + { + "name": "edge", + "type_name": "igraphEdge", + "description": "Used to represent an edge in the graph. It has attributes such as `origin`, `destination`, `attrs`, and `value`." + } + ], + "returns": null, + "name": "insert_igraph_edge", + "location": { + "start": 532, + "insert": 535, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 7, + "docLength": null + }, + { + "id": "a24f000e-1b16-fe81-7549-3dd2b8700dc0", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Modifies an existing edge in a graph based on information from two nodes, adding new attributes for the RT translation and rotation of the edge.", + "params": [ + { + "name": "org", + "type_name": "Agent", + "description": "Used to specify the origin node of the edge to be inserted." + }, + { + "name": "dest", + "type_name": "Agent", + "description": "Used to represent the destination node or edge in the graph." + } + ], + "returns": null, + "name": "insert_dsr_edge", + "location": { + "start": 544, + "insert": 547, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 22, + "docLength": null + }, + { + "id": "2e01c904-f5c2-73b3-6a47-71c9916a753a", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Clears the axis, layouts the graph using Kamada-Kawai layout algorithm, and then plots the vertices and edges with grey lines and arrowheads pointing towards the nodes. It also displays node names using text annotation.", + "params": [], + "returns": null, + "name": "draw_graph", + "location": { + "start": 574, + "insert": 575, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 16, + "docLength": null + }, + { + "id": "5f26e45d-f00c-1b98-1f4b-5af6c11f2db2", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Determines the room ID of an element with the given node ID, retrieving the attribute value from the node's attributes and returning it if successful, or -1 otherwise.", + "params": [ + { + "name": "node_id", + "type_name": "int", + "description": "Passed as an argument to the function representing a unique identifier for an element in the graph." + } + ], + "returns": { + "type_name": "int", + "description": "The room ID corresponding to the given node ID when the node has the \"room_id\" attribute, or -1 otherwise." + }, + "name": "check_element_room_number", + "location": { + "start": 599, + "insert": 600, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 7, + "docLength": null + }, + { + "id": "af524d88-6016-9cb4-f54b-329cbba2d34f", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Within the SpecificWorker class checks the \"level\" attribute of a node and returns its value if found, or -1 otherwise. It also performs additional actions related to robot position and room connections in the internal graph.", + "params": [ + { + "name": "node_id", + "type_name": "str", + "description": "Used to represent the unique identifier of a node in the graph." + } + ], + "returns": { + "type_name": "int", + "description": "The level of an element with the given node ID." + }, + "name": "check_element_level", + "location": { + "start": 608, + "insert": 609, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 25, + "docLength": null + }, + { + "id": "5fe9df41-dec5-989d-6c49-d290d3b59d9c", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Retrieves information about a room node in a graph, specifically its ID and the IDs of its adjacent nodes with a certain edge type, and then prints out these values and attempts to retrieve a translation attribute.", + "params": [ + { + "name": "room_node_id", + "type_name": "str", + "description": "Used to get a specific node from the graph." + } + ], + "returns": null, + "name": "generate_room_picture", + "location": { + "start": 651, + "insert": 653, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 24, + "docLength": null + }, + { + "id": "8f2759df-b075-8896-614a-8578e1b2c573", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Creates a new edge in the graph with the current room ID and assigns it to the `self.g` variable, inserting or assigning the edge if it does not already exist.", + "params": [ + { + "name": "room_id", + "type_name": "str", + "description": "Provided as an argument to the function by the caller, representing the room ID that the current edge is associated with." + } + ], + "returns": null, + "name": "insert_current_edge", + "location": { + "start": 686, + "insert": 688, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 3, + "docLength": null + }, + { + "id": "68534604-01c8-c8b5-fe4b-df8ff1274feb", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Updates the state of an affordance node based on its ID and type. If the active affordance node's state is \"completed\" and its active status is false, it transitions to the \"crossed\" state.", + "params": [ + { + "name": "id", + "type_name": "int", + "description": "Representing the unique identifier for the node to be updated." + }, + { + "name": "type", + "type_name": "str", + "description": "Used to indicate the type of the node being updated, which can be either \"active\" or \"completed\"." + } + ], + "returns": null, + "name": "update_node", + "location": { + "start": 700, + "insert": 701, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 9, + "docLength": null + }, + { + "id": "6d818941-6870-1f92-724a-6a32635de3b4", + "ancestors": [ + "76fa4ef8-3f83-f299-c341-308421fee7c7" + ], + "description": "Updates the information about the robot's edge when it moves to a new node and the type of the edge is RT(room transition).", + "params": [ + { + "name": "fr", + "type_name": "int", + "description": "The ID of the room exit door." + }, + { + "name": "to", + "type_name": "int", + "description": "Passed to identify the destination node for updating the edge." + }, + { + "name": "type", + "type_name": "str", + "description": "Representing the type of edge to be updated, which can be either \"RT\" or \"PT\"." + } + ], + "returns": null, + "name": "update_edge", + "location": { + "start": 715, + "insert": 719, + "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..340f5f80 --- /dev/null +++ b/.komment/komment.json @@ -0,0 +1,21 @@ +{ + "meta": { + "version": "1", + "updated_at": "2024-07-03T15:48:26.896Z", + "created_at": "2024-07-03T15:48:27.349Z", + "pipelines": [ + "50c96b7c-ad3b-4b93-ae19-7f6fbd5637f7" + ] + }, + "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" + ] + ] +} \ 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..d53efd4b 100644 --- a/agents/g2o_agent/src/specificworker.py +++ b/agents/g2o_agent/src/specificworker.py @@ -48,7 +48,126 @@ pass class SpecificWorker(GenericWorker): + """ + Performs real-time visual odometry estimation for a robot using a G2O graph + and RT communication. It initializes a graph, adds nominal corners, and updates + edges with measured data from an RT communication agent. + + Attributes: + Period (float): Used to control the interval at which the worker performs + its tasks, with a default value of 1 second. It can be used to adjust + the worker's execution speed. + agent_id (int): A unique identifier assigned to each agent in the system. + g (Graph): Used for representing the graph of a robot's motion, where each + node represents a pose of the robot and each edge represents a motion + of the robot between two poses. The graph is used to compute the + marginal probability distribution of the robot's motion given its + initial position and velocity. + startup_check (QTimersingleShot): Used to start a quit timer after 200 + milliseconds of inactivity, which allows the worker to exit gracefully + when no further work is available. + rt_api (str): 8 characters long. It stores a string value that represents + the ROS launch file path for the RT node. + inner_api (instance): Used as a reference to the inner API object, which + is responsible for handling the actual work of the worker. + odometry_node_id (int): 0-based index of the node representing the robot's + odometry information in the graph. It is used to identify the node in + the graph that represents the robot's current position and velocity. + odometry_queue (3D): A queue that stores the odometry data of the robot, + which is used to update the graph and calculate the robot's pose. + last_odometry (3D): Used to store the last known odometry value of the + robot, which is used for calculating the displacement and covariance + matrix. + g2o (3D): A G2O graph, which is a data structure used to represent the + robot's pose in a 3D environment. It stores the information of the + robot's pose in a set of nodes and edges, where each node represents + a location in space and each edge represents a connection between two + locations. The `g2o` attribute is used to perform optimization tasks + such as finding the robot's pose that minimizes a cost function. + odometry_noise_std_dev (float): Used to represent the standard deviation + of odometry noise in the worker's graph. It is used in the `get_displacement` + method to compute the displacement between two poses based on the + odometry noise. + odometry_noise_angle_std_dev (floatingpoint): 1.5 by default, which + represents the standard deviation of angle noise in the odometry + measurement of a robot. It affects the accuracy of the robot's positioning + and movement tracking. + measurement_noise_std_dev (float): 10% of the robot's minimum distance + from its reference configuration to its target position, indicating + the standard deviation of measurement noise in the robot's pose estimation. + last_room_id (int): Used to store the previous room ID that was visited + by the agent before the current room. It is updated when the agent + moves to a new room, allowing the worker to detect changes in the environment. + actual_room_id (int): Used to store the current room ID of the agent, which + is updated when the robot moves from one room to another. + elapsed (int): Used to store the time elapsed since the last call to the + `startup_check` function, which is called every 0.1 seconds. It is + used to determine when to quit the application. + room_initialized (bool): Used to keep track of whether the current room + has been initialized or not. It's set to False when a new room is + entered, and True otherwise. + iterations (int): Used to keep track of the number of iterations performed + by the worker. It increases by one each time a new iteration is performed + and can be accessed or modified from within the worker's methods. + hide (str): Used to specify whether a worker should be hidden or not when + multiple workers are running simultaneously. + init_graph (bool): Used to indicate whether the graph has been initialized + or not. It is set to True when the graph is initialized and False + otherwise, allowing the worker to distinguish between initialized and + uninitialized graphs. + current_edge_set (bool): Used to indicate whether the current edge set has + been computed or not. It is set to true when the edge set is first + computed and false otherwise, so that only new computations are performed. + first_rt_set (bool): Set to `True` when a new RT translation and rotation + are detected, and `False` otherwise. It indicates whether the worker + has received its first RT set or not. + translation_to_set (3D): Used to store the translation from the robot's + reference frame to the set frame. + rotation_to_set (3D): Used to store the rotation of a robot's end effector + relative to its base frame, which is necessary for setting the desired + orientation of the robot's end effector. + room_polygon (numpyarray): Used to store the coordinates of a room's + boundary in a list of (x,y) tuples. + initialize_g2o_graph (instance): Used to initialize a Graph2ONode graph + for a specific robotic arm. It creates nodes, edges, and defines the + room node, and sets up the optimization algorithm and visualization tools. + rt_set_last_time (float): Used to store the last time when a RT set was + set by the worker, which is used to filter out unnecessary RT sets. + rt_time_min (float): Set to the minimum time between two consecutive RT + sets. It is used to determine when to reset the translation and rotation + values for RT tracking purposes. + timer (QTimer): Used to schedule periodic updates of the graph with the + current robot state. It is set to single shot every 200 milliseconds, + allowing for real-time updates of the graph. + compute (instance): Used to compute the marginal probability distributions + of the robot's state given its current observations. It takes an + argument `vertex` which is a vertex in the graph representing the + robot's state, and returns a tuple containing the marginal probabilities + and the Hessian matrix of the robot's state. + update_node_att (update): Called when a new node with the specified id is + added to the graph, or an existing node's attributes are updated. It + updates the odometry queue based on the new node's attributes and sets + the room initialization flag accordingly. + update_edge (edge): Used to update the attributes of an edge in a graph + when the edge's node types change or when a new edge is added to the + graph. It sets the `rt_translation` and `rotation_euler_xyz` attributes + of the edge based on the new node types. + update_edge_att (attribute): Used to update the attributes of an edge in + a graph. It takes the edge ID, the new value for the attribute, and + the name of the attribute as input parameters. + + """ def __init__(self, proxy_map, startup_check=False): + """ + Initializes a SpecificWorker instance, defining its attributes and connections + to graphs, timers, and signal handlers. + + Args: + proxy_map (dict): Used to store a mapping from agent IDs to graphs. + startup_check (Optionalbool): Used to check if the robot's graph has + already been initialized before starting the main loop. + + """ super(SpecificWorker, self).__init__(proxy_map) self.Period = 50 @@ -119,6 +238,13 @@ def setParams(self, params): @QtCore.Slot() def compute(self): + """ + Generates high-quality summaries of Java code that is given to it. It takes + into account the robot's odometry, computes the translation and rotation + of the robot relative to a reference frame, and adds the information to + the graph. It also updates the landmark in the graph. + + """ if time.time() - self.elapsed > 1: print("Frame rate: ", self.iterations, " fps") self.elapsed = time.time() @@ -212,6 +338,16 @@ def add_noise(self, value, std_dev): return value + np.random.normal(0, std_dev) def initialize_g2o_graph(self): + """ + Initializes a Graph2O graph for a specific robot and room, adding nominal + corners and measuring them to create a polygonal representation of the + room. It also sets the robot's initial position and orientation in the graph. + + Returns: + bool: 1 if the g2o graph was successfully initialized with the given + room id, and 0 otherwise. + + """ 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: @@ -283,6 +419,20 @@ def initialize_g2o_graph(self): return False def get_displacement(self, odometry): + """ + Calculates the displacement of a robot based on its odometry data, updating + the robot's position and orientation using the lateral, angular, and linear + displacements. + + Args: + odometry (stdvectordouble): 3D vector representing the robot's position, + orientation, and velocity at a given time stamp. + + Returns: + triplet: 3 elements long (desplazamiento lateral, displacement avance + and angular displacement). + + """ desplazamiento_avance = 0 desplazamiento_lateral = 0 desplazamiento_angular = 0 @@ -302,6 +452,21 @@ def get_displacement(self, odometry): return desplazamiento_lateral, desplazamiento_avance, desplazamiento_angular def get_covariance_matrix(self, vertex): + """ + Computes the covariance matrix of a set of vertices in a graph using the + G2O optimization algorithm. It takes a vertex as input, returns the computed + covariance matrix and a result flag indicating whether the computation was + successful. + + Args: + vertex (g2oVertex): Passed to compute marginals for computing covariance + matrix. + + Returns: + tuple: 2-element, containing the result of computing covariance matrix + and the marginals of the vertices. + + """ cov_vertices = [(vertex.hessian_index(), vertex.hessian_index())] covariances, covariances_result = self.g2o.optimizer.compute_marginals(cov_vertices) if covariances_result: @@ -315,6 +480,15 @@ def get_covariance_matrix(self, vertex): return (covariances_result, None) def visualize_g2o_realtime(self, optimizer): + """ + Visualizes a G2O optimization problem in real-time, loading the problem + from an file and plotting the vertices and edges as 3D scatter plots while + updating the display with small intervals of time. + + Args: + optimizer (instance): Used to load G2O files for visualization in real-time. + + """ plt.ion() fig = plt.figure() ax = fig.add_subplot(111, projection='3d') @@ -360,6 +534,16 @@ def update_node_att(self, id: int, attribute_names: [str]): # # self.init_graph = True # print("INIT GRAPH") + """ + Updates the attributes of a node in a graph, specifically the `odometry_queue` + attribute, when the node's ID matches the `odometry_node_id`. + + Args: + id (int): Used to identify the node for which attribute values are + being updated. + attribute_names ([str]): A list of attribute names to update in the node. + + """ if id == self.odometry_node_id: odom_node = self.g.get_node("Shadow") odom_attrs = odom_node.attrs @@ -377,9 +561,24 @@ 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 the node with the specified ID, based on the type of node it represents. + + Args: + id (int): Used to identify the node being updated. + type (str): Used to specify the node type. + + """ pass def delete_node(self, id: int): + """ + Deletes a node from the graph represented by the `SpecificWorker` class. + + Args: + id (int): Used to identify the node to be deleted. + + """ pass # if type == "room": # # TODO: reset graph and wait until new room appears @@ -389,6 +588,17 @@ 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 room ID and sets the current edge set based on the provided + edge type and node types. + + Args: + fr (int): Representative of the start node of an edge in a graph. + to (int): Used to represent the target node of an edge in the graph. + type (str): Used to specify the type of edge to update, either "current" + or "RT". + + """ if type == "current" and self.g.get_node(fr).type == "room": self.room_initialized = False # Get number after last "_" in room name @@ -417,5 +627,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): + """ + Deletes an edge from a graph, specified by the vertex indices `fr` and + `to`, and the edge type. + + Args: + fr (int): Used to represent the starting vertex index of an edge to + be deleted. + to (int): Used to indicate the destination node ID for edge deletion. + type (str): Used to specify the edge type to be deleted, with possible + values 'in' or 'out'. + + """ 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..9e30e24e 100644 --- a/agents/long_term_spatial_memory_agent/src/genericworker.py +++ b/agents/long_term_spatial_memory_agent/src/genericworker.py @@ -42,9 +42,34 @@ class GenericWorker(QtWidgets.QWidget): + """ + Provides a mechanism for killing itself and setting a period for a timer. It + has a signal `kill` that is emitted when the object is killed, and a method + `setPeriod` to set the timer period. + + Attributes: + kill (QtCoreSignal): Used to signal the object's termination. + ui (Ui_guiDlg): Used to store the user interface of a GUI dialog box. + mutex (QMutex): Used to protect the internal state of the worker object. + Period (int): 500 milliseconds by default, used to set the time interval + for the timer event emitted by the `setPeriod()` method. + timer (QtCoreQTimer): Used to start a timer that calls the `setPeriod` + slot when it expires. + + """ kill = QtCore.Signal() def __init__(self, mprx): + """ + Initializes an instance of the `GenericWorker` class by setting up the UI, + creating a mutex for thread-safe access to the `Period` variable, and + starting a timer with a 500ms delay. + + Args: + mprx (Ui_guiDlg): Used to set up the user interface for the GenericWorker + class. + + """ super(GenericWorker, self).__init__() @@ -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 prints a message to the console when it changes. + + Args: + p (int): Used to set the new period value for the application's 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..ac88cd3c 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,7 +13,38 @@ class LongTermGraph: + """ + Performs a series of actions: it reads and computes a graph from a text file, + draws the graph using Matplotlib, and provides methods for navigating and + querying the graph. + + Attributes: + g (undetected): Used to store the graph data structure for computing the + metric reconstruction. + read_graph (method): Responsible for reading a graph from a file in Graphviz + format. It takes no arguments and returns a Graph object. + fig (matplotlibfigureFigure): Used to represent the figure object for + visualizing the graph. + ax (PyQt5QtWidgetsQGraphicsScene): Used to represent the graphical display + area where the graph will be drawn. + fig_2 (matplotlibfigureFigure): Used to store the figure object for the + graph display. + ax_2 (matplotlibFigure): Used to store a second axis object for plotting + the graph. + + """ def __init__(self, file_name): + """ + Reads a graph from a file, creates two subplots in an matplotlib figure + for visualizing the graph and its metric reconstruction, and sets up labels + for the x- and y-axes. If the file is not found, it sets `self.g` to `None`. + + Args: + file_name (str): Used to specify the path of a graph file that contains + a directed multigraph, which will be read by the function and + processed for metric reconstruction. + + """ try: self.g = self.read_graph(file_name, directed=True) print("Graph read from", file_name, self.g.summary()) @@ -212,6 +243,16 @@ 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 of a larger graph, based + on node and edge attributes. It creates a scatter plot of room and door + nodes, and plots edges as grey lines with node names annotated. + + Args: + only_rooms (bool): Used to filter out nodes that are not rooms, doors, + or walls. + + """ self.ax_2.clear() self.ax_2.set_title('LTSM graph') self.ax_2.set_xlabel('X-axis') diff --git a/agents/long_term_spatial_memory_agent/src/specificworker.py b/agents/long_term_spatial_memory_agent/src/specificworker.py index 634d6ae7..b15ebdac 100644 --- a/agents/long_term_spatial_memory_agent/src/specificworker.py +++ b/agents/long_term_spatial_memory_agent/src/specificworker.py @@ -47,7 +47,89 @@ from pydsr import * class SpecificWorker(GenericWorker): + """ + Is responsible for processing specific types of nodes and edges in a graph, + such as room nodes and their corresponding doors, as well as affordance nodes + and their transitions. It also handles updating the graph's attributes and + inserting new edges. + + Attributes: + Period (float32): 0 by default. It represents the period of time taken for + a specific worker to complete its task, which can be used to determine + the next task to assign to the worker based on its current state. + agent_id (int): 0 by default, representing the agent's internal ID for + distinguishing it from other agents. + g (Graph): Used to store the graph data structure for the environment map + navigation. It contains the nodes, edges, and other attributes needed + for the worker's functionality. + update_node (attribute): Called when a node's attributes change. It updates + the state of the worker based on the new attributes. + update_edge (Edge): Used to update the edge attributes based on the robot's + movement. It takes three parameters: `fr`, `to`, and `type`, which + represent the from, to, and edge type respectively. The method updates + the edge attributes accordingly and sets the current edge as "current" + if there are no existing "current" edges. + startup_check (QTimersingleShot): Used to schedule the worker's main loop + to run after 200 milliseconds of starting the application. + rt_api (Attribute): Used to store the RT API of the worker, which is needed + for communication between the worker and the main agent. + inner_api (internal): Used by the worker to interact with its inner agent + API, which handles the actual robot control and sensor data processing. + robot_name (str): Used to store the name of the robot that the worker is + controlling. + robot_id (int): Used to store the ID of the robot that the worker represents. + last_robot_pose (Attribute): Used to store the last known pose of the robot + in the environment, which can be used for navigation and task completion + purposes. + state (str): Used to keep track of the worker's current state, either + "idle", "running", or "crossed". + affordance_node_active_id (int): Used to store the ID of the affordance + node that is currently active, indicating which affordance the worker + should perform. + exit_door_id (int): 0-based, representing the index of the door through + which the robot exits a room when it moves from that room to another. + room_exit_door_id (int): 14 by default. It represents the ID of the door + through which the robot exits a room. + enter_room_node_id (int): Used to store the ID of the room node that the + robot enters when it moves from the previous room to the current room. + graph (Graph): Used to store the graph representation of the environment, + including nodes, edges, and their attributes. + vertex_size (int): Used to represent the size of a vertex in a graph, + indicating its importance or relevance in the graph structure. + not_required_attrs (list): Used to store the names of attributes that are + not required for the worker's functionality, i.e., they are optional + or not used in the implementation. + long_term_graph (Graph): Used to store the long-term graph of the agent, + which is updated at each iteration of the worker. It keeps track of + the agent's internal graph over time, allowing the worker to perform + actions based on the historical information of the environment. + global_map (ndarray): 2D, representing a map of the environment. It stores + the occupancy probabilities of each cell in the grid, which are used + to calculate the worker's attention and motion. + insert_current_edge (Edge): Used to insert a new edge into the graph with + a specified type (current) and source and destination nodes, signaling + that the current room has been reached and the agent should move on + to the next task. + timer (QTimer): Used to schedule the worker's startup check after a delay + of 200 milliseconds. + compute (instance): Used to compute the next action for the agent based + on the current state of the environment. It takes into account the + agent's attributes, such as its position and orientation, and returns + a dict with the next action as key and the corresponding value as value. + + """ def __init__(self, proxy_map, startup_check=False): + """ + Sets up the internal structure and event connections for a SpecificWorker + instance, including its graph, node ID, and timer for computing updates. + + Args: + proxy_map (igraphGraph): Used to specify the graph for this specific + worker instance. + startup_check (bool): Used to check if any of the required nodes are + present in the graph before starting the agent's inner loop. + + """ super(SpecificWorker, self).__init__(proxy_map) self.Period = 100 @@ -113,6 +195,18 @@ def __del__(self): """Destructor""" def setParams(self, params): + """ + Sets parameters and updates node attributes in a graph. It removes a + self-edge from a room, stores an exit door ID, and assigns other_side_door + attributes to both doors leading to the new room. + + Args: + params (object): Passed to the function for configuration purposes. + + Returns: + Boolean: True. + + """ return True # PROTOcode @@ -145,6 +239,12 @@ def compute(self): # if origin_level is not None and destination_level is not None: # print(origin_level, destination_level) + """ + Performs various actions based on the current state of the worker, including + idle, crossing, crossed, initializing room, known room, initializing doors, + storing graph, and removing. + + """ match self.state: case "idle": self.idle() @@ -166,6 +266,11 @@ def compute(self): 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 + """ + Determines if the agent is entering or exiting a room based on the affordance + graph and updates the agent's state and door associations accordingly. + + """ 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: @@ -257,6 +362,11 @@ def idle(self): def crossed(self): # Get parent node of affordance node + """ + Determines if the agent has reached a door and, if so, updates its state + based on the door's properties. + + """ affordance_node = self.g.get_node(self.affordance_node_active_id) if not affordance_node.attrs["parent"].value: # print("Affordance node has no parent") @@ -278,6 +388,11 @@ def crossed(self): def initializing_room(self): # Get room nodes + """ + Determines and stores the ID of the room node to enter when it is called, + and updates the state of the worker 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") @@ -291,6 +406,14 @@ def initializing_room(self): # def known_room(self): # Get other side door name attribute + """ + Performs a series of actions to handle a robot's movement through a door, + including: + 1/ Finding the other side room node in the graph. + 2/ Creating a new edge and vertex for the robot's new location. + 3/ Deleting the original edge connecting the robot to the room. + + """ 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 @@ -361,6 +484,12 @@ def known_room(self): def initializing_doors(self): # Check if node called "room_entry" of type room exists + """ + 1) retrieves exit edges and matching same-side doors, 2) sets other side + door name and connected room name attributes for each exit door node, and + 3) associates doors between nodes using their names. + + """ 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 @@ -400,6 +529,18 @@ def initializing_doors(self): def associate_doors(self, door_1, door_2): # Find each door in igraph and update attributes + """ + Takes two door nodes as input and connects them by adding an edge between + them, while also updating the "other side door name" and "connected room + name" attributes of each node to reflect the connection. + + Args: + door_1 (str): Represented as an tuple containing the name of the door + and its room, such as ("Door 1", "Room 1"). + door_2 (str): Represented as a string containing the name of the second + door to be associated with the graph. + + """ try: door_1_node = self.graph.vs.find(name=door_1[0]) except: @@ -418,6 +559,11 @@ def associate_doors(self, door_1, door_2): def store_graph(self): + """ + Stores the graph represented by an instance of `GenericWorker` in a file + named "graph.pkl". + + """ actual_room_node = self.g.get_node(self.room_exit_door_id) # Check if node in igraph with the same name exists try: @@ -433,6 +579,13 @@ def store_graph(self): def removing(self): # # Get last number in the name of the room + """ + Removes edges from the graph that connect two nodes with the same room + number, and also deletes nodes that have only one connection to another + node with a room number of 200 (shadow nodes). The function then updates + the long-term graph and sets the state to "idle". + + """ 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") @@ -459,6 +612,14 @@ def removing(self): def traverse_graph(self, node_id): # Mark the current node as visited and print it + """ + Iterates through the robot's graph, identifying and traversing RT edges, + and inserting vertices and edges into an IGraph object. + + Args: + node_id (int): A unique identifier for the node being 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) @@ -468,6 +629,16 @@ def traverse_graph(self, node_id): self.insert_igraph_edge(i) def traverse_igraph(self, node): + """ + Iterates through the successors of a given vertex in a graph, and inserts + vertices and edges into a data structure called `dsr` if the successor has + a higher level than the current vertex and its name is not already present + in `dsr`. + + Args: + node (igraphVertex): Represented by an index. + + """ vertex_successors = self.graph.successors(node.index) # Recur for all the vertices adjacent to thisvertex for i in vertex_successors: @@ -481,6 +652,14 @@ def traverse_igraph(self, node): continue def insert_igraph_vertex(self, node): + """ + Adds a new vertex to an igraph graph, updating its attributes and linking + it with other vertices based on their names. + + Args: + node (igraphNode): Passed the vertex to be added to the graph. + + """ 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: @@ -517,6 +696,19 @@ def insert_igraph_vertex(self, node): def insert_dsr_vertex(self, parent_name, node): # print("Inserting vertex", node["name"], node["type"]) + """ + Creates a new node in the graph and links it to its parent node by setting + the `parent` attribute. It also sets other attributes based on the input + node data and inserts the new node into the graph using the `insert_node` + method. + + Args: + parent_name (str): Representative of the name of a node to which a new + vertex will be added as its parent. + node (Node): Passed as an instance of the class representing a graph + node, containing attributes such as agent ID and type. + + """ new_node = Node(agent_id=self.agent_id, type=node["type"], name=node["name"]) # Check if the node is a room node @@ -532,6 +724,15 @@ def insert_dsr_vertex(self, parent_name, node): def insert_igraph_edge(self, edge): # Search for the origin and destination nodes in the graph + """ + Adds an edge to an igraph graph, specifying the origin and destination + nodes, as well as the rotation and translation attributes of the edge. + + Args: + edge (igraphEdge): Used to represent an edge in the graph. It has + attributes such as `origin`, `destination`, `attrs`, and `value`. + + """ origin_node = self.graph.vs.find(id=edge.origin) destination_node = self.graph.vs.find(id=edge.destination) # Add the edge to the graph @@ -544,6 +745,15 @@ def insert_igraph_edge(self, edge): def insert_dsr_edge(self, org, dest): # print("ORG::", org) # self.insert_dsr_vertex(dest) + """ + Modifies an existing edge in a graph based on information from two nodes, + adding new attributes for the RT translation and rotation of the edge. + + Args: + org (Agent): Used to specify the origin node of the edge to be inserted. + dest (Agent): Used to represent the destination node or edge in the graph. + + """ if org is None: root_node = self.g.get_node("root") org_id = root_node.id @@ -572,6 +782,12 @@ def insert_dsr_edge(self, org, dest): self.g.insert_or_assign_edge(new_edge) def draw_graph(self): + """ + Clears the axis, layouts the graph using Kamada-Kawai layout algorithm, + and then plots the vertices and edges with grey lines and arrowheads + pointing towards the nodes. It also displays node names using text annotation. + + """ self.ax.clear() # Obtener las coordenadas de los vértices layout = self.graph.layout("kamada_kawai") # Utiliza el layout Kamada-Kawai @@ -597,6 +813,20 @@ def draw_graph(self): self.ax.set_ylim([min(y) - 2, max(y) + 2]) def check_element_room_number(self, node_id): + """ + Determines the room ID of an element with the given node ID, retrieving + the attribute value from the node's attributes and returning it if successful, + or -1 otherwise. + + Args: + node_id (int): Passed as an argument to the function representing a + unique identifier for an element in the graph. + + Returns: + int: The room ID corresponding to the given node ID when the node has + the "room_id" attribute, or -1 otherwise. + + """ node = self.g.get_node(node_id) try: room_id = node.attrs["room_id"].value @@ -606,6 +836,19 @@ def check_element_room_number(self, node_id): return -1 def check_element_level(self, node_id): + """ + Within the SpecificWorker class checks the "level" attribute of a node and + returns its value if found, or -1 otherwise. It also performs additional + actions related to robot position and room connections in the internal graph. + + Args: + node_id (str): Used to represent the unique identifier of a node in + the graph. + + Returns: + int: The level of an element with the given node ID. + + """ node = self.g.get_node(node_id) try: element_level = node.attrs["level"].value @@ -650,6 +893,15 @@ def check_element_level(self, node_id): def generate_room_picture(self, room_node_id): + """ + Retrieves information about a room node in a graph, specifically its ID + and the IDs of its adjacent nodes with a certain edge type, and then prints + out these values and attempts to retrieve a translation attribute. + + Args: + room_node_id (str): Used to get a specific node from the graph. + + """ room_node = self.g.get_node(room_node_id) # Get room node room_id attribute room_id = room_node.attrs["room_id"].value @@ -685,6 +937,16 @@ def generate_room_picture(self, room_node_id): def insert_current_edge(self, room_id): # Insert current edge to the room + """ + Creates a new edge in the graph with the current room ID and assigns it + to the `self.g` variable, inserting or assigning the edge if it does not + already exist. + + Args: + room_id (str): Provided as an argument to the function by the caller, + representing the room ID that the current edge is associated with. + + """ current_edge = Edge(room_id, room_id, "current", self.agent_id) self.g.insert_or_assign_edge(current_edge) @@ -698,6 +960,17 @@ 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 state of an affordance node based on its ID and type. If the + active affordance node's state is "completed" and its active status is + false, it transitions to the "crossed" state. + + Args: + id (int): Representing the unique identifier for the node to be updated. + type (str): Used to indicate the type of the node being updated, which + can be either "active" or "completed". + + """ if id == self.affordance_node_active_id: print("Affordance node is active") affordance_node = self.g.get_node(id) @@ -716,6 +989,17 @@ 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 information about the robot's edge when it moves to a new node + and the type of the edge is RT(room transition). + + Args: + fr (int): The ID of the room exit door. + to (int): Passed to identify the destination node for updating the edge. + type (str): Representing the type of edge to be updated, which can be + either "RT" or "PT". + + """ 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: From 6924a8f47f57c896237086b49affd2ef0169423b Mon Sep 17 00:00:00 2001 From: "komment-ai[bot]" <122626893+komment-ai[bot]@users.noreply.github.com> Date: Thu, 4 Jul 2024 16:02:13 +0000 Subject: [PATCH 02/10] Added comments to 4 functions across 1 file --- .komment/00000.json | 38 +++++++++---------- .komment/komment.json | 5 ++- .../src/genericworker.py | 36 +++++++++--------- 3 files changed, 40 insertions(+), 39 deletions(-) diff --git a/.komment/00000.json b/.komment/00000.json index 2c1dfcb3..df7abc9a 100644 --- a/.komment/00000.json +++ b/.komment/00000.json @@ -1022,37 +1022,37 @@ "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 for killing itself after a specified period. The code also includes Ice loadSlice command to load an Ice slice file, imports RoboCompCommonBehavior module, and sets up a GUI using PySide2.", + "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": "19efd488-1ac0-7f99-eb42-0c5514f4d762", + "id": "8a50128c-009f-a1ac-2c45-674f0e62438e", "ancestors": [], - "description": "Provides a mechanism for killing itself and setting a period for a timer. It has a signal `kill` that is emitted when the object is killed, and a method `setPeriod` to set the timer period.", + "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": "QtCoreSignal", - "description": "Used to signal the object's termination." + "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 store the user interface of a GUI dialog box." + "description": "Used to setup the user interface of the class." }, { "name": "mutex", "type_name": "QMutex", - "description": "Used to protect the internal state of the worker object." + "description": "Used to protect the worker's state from concurrent access." }, { "name": "Period", "type_name": "int", - "description": "500 milliseconds by default, used to set the time interval for the timer event emitted by the `setPeriod()` method." + "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 calls the `setPeriod` slot when it expires." + "description": "Used to start a timer that emits the `kill` signal after a specified period." } ], "name": "GenericWorker", @@ -1068,16 +1068,16 @@ "docLength": null }, { - "id": "47585d96-d782-50be-9d43-a6041210bddd", + "id": "dc756aa5-ebc6-5490-d649-854bb2ac26a4", "ancestors": [ - "19efd488-1ac0-7f99-eb42-0c5514f4d762" + "8a50128c-009f-a1ac-2c45-674f0e62438e" ], - "description": "Initializes an instance of the `GenericWorker` class by setting up the UI, creating a mutex for thread-safe access to the `Period` variable, and starting a timer with a 500ms delay.", + "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 to set up the user interface for the GenericWorker class." + "description": "Used as the parent widget for the GenericWorker object's UI." } ], "returns": null, @@ -1094,9 +1094,9 @@ "docLength": null }, { - "id": "ad8b787d-cd38-c9a5-2048-7db5d71e901b", + "id": "17fccd3d-bcc7-a9a3-5f45-73125f7d1f78", "ancestors": [ - "19efd488-1ac0-7f99-eb42-0c5514f4d762" + "8a50128c-009f-a1ac-2c45-674f0e62438e" ], "description": "Emits the `kill` signal, indicating that the object should be terminated.", "params": [], @@ -1114,16 +1114,16 @@ "docLength": null }, { - "id": "f5aaf03a-bb47-4a92-7f4d-501fec240a0e", + "id": "a2ebb705-7932-fb9b-3e45-f965c61ebf4d", "ancestors": [ - "19efd488-1ac0-7f99-eb42-0c5514f4d762" + "8a50128c-009f-a1ac-2c45-674f0e62438e" ], - "description": "Sets the period of a timer and prints a message to the console when it changes.", + "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 value for the application's timer." + "description": "Used to set the new period for the timer." } ], "returns": null, diff --git a/.komment/komment.json b/.komment/komment.json index 340f5f80..0db52d3d 100644 --- a/.komment/komment.json +++ b/.komment/komment.json @@ -1,10 +1,11 @@ { "meta": { "version": "1", - "updated_at": "2024-07-03T15:48:26.896Z", + "updated_at": "2024-07-04T16:02:07.320Z", "created_at": "2024-07-03T15:48:27.349Z", "pipelines": [ - "50c96b7c-ad3b-4b93-ae19-7f6fbd5637f7" + "50c96b7c-ad3b-4b93-ae19-7f6fbd5637f7", + "9e4d0253-62f2-4920-b2d3-607cb4cf3291" ] }, "lookup": [ diff --git a/agents/long_term_spatial_memory_agent/src/genericworker.py b/agents/long_term_spatial_memory_agent/src/genericworker.py index 9e30e24e..c78d937a 100644 --- a/agents/long_term_spatial_memory_agent/src/genericworker.py +++ b/agents/long_term_spatial_memory_agent/src/genericworker.py @@ -43,37 +43,37 @@ class GenericWorker(QtWidgets.QWidget): """ - Provides a mechanism for killing itself and setting a period for a timer. It - has a signal `kill` that is emitted when the object is killed, and a method - `setPeriod` to set the timer period. + 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 (QtCoreSignal): Used to signal the object's termination. - ui (Ui_guiDlg): Used to store the user interface of a GUI dialog box. - mutex (QMutex): Used to protect the internal state of the worker object. - Period (int): 500 milliseconds by default, used to set the time interval - for the timer event emitted by the `setPeriod()` method. - timer (QtCoreQTimer): Used to start a timer that calls the `setPeriod` - slot when it expires. + 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 instance of the `GenericWorker` class by setting up the UI, - creating a mutex for thread-safe access to the `Period` variable, and - starting a timer with a 500ms delay. + 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 to set up the user interface for the GenericWorker - class. + 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() @@ -96,10 +96,10 @@ def killYourSelf(self): @QtCore.Slot(int) def setPeriod(self, p): """ - Sets the period of a timer and prints a message to the console when it changes. + Sets the period of a timer and updates the internal variable `Period`. Args: - p (int): Used to set the new period value for the application's timer. + p (int): Used to set the new period for the timer. """ print("Period changed", p) From 5a781b55c4f2a93ff7d99d1a892a662c2f46a9e4 Mon Sep 17 00:00:00 2001 From: "komment-ai[bot]" <122626893+komment-ai[bot]@users.noreply.github.com> Date: Mon, 8 Jul 2024 15:05:38 +0000 Subject: [PATCH 03/10] Added comments to 40 functions across 3 files --- .komment/00000.json | 677 +++++++++--------- .komment/komment.json | 5 +- agents/g2o_agent/src/specificworker.py | 581 +++++++++------ .../src/long_term_graph.py | 74 +- .../src/specificworker.py | 527 +++++++------- 5 files changed, 1042 insertions(+), 822 deletions(-) diff --git a/.komment/00000.json b/.komment/00000.json index df7abc9a..fdd68d78 100644 --- a/.komment/00000.json +++ b/.komment/00000.json @@ -4,42 +4,42 @@ "path": "agents/long_term_spatial_memory_agent/src/long_term_graph.py", "content": { "structured": { - "description": "A class `RoomFrame` that represents a 3D room frame and its corresponding objects (walls, floors, ceilings, doors, and windows) in a graph structure. The code provides methods for drawing the room frame and its objects on an axis-aligned bounding box (AABB) in 2D or 3D space using Matplotlib. The `get_room_corners` method recursively traverses the graph to compute the projective coordinates of the corners of a room, while the `get_room_objects_coordinates` method computes the coordinates of objects of a specific type within a room. Both methods return lists of 4D vectors representing the position and orientation of each object or corner in the room frame's coordinate system.", + "description": "A 2D environment with obstacles and a player, and uses a probabilistic algorithm to compute the shortest path between two points in the environment while avoiding obstacles. It also provides functions to draw the environment and its objects on a figure using Matplotlib, and to compute the projective coordinates of the corners of a room in the environment frame. Additionally, it defines functions to get the list of objects in a room by type and their projective coordinates in the room frame. The code uses the Geometry package for computing shortest paths and the Matplotlib library for drawing visualizations.", "items": [ { - "id": "8aa24592-d2b2-699c-8b4a-818d3e97115b", + "id": "7453d1a9-2093-7088-9941-e01ea3c40300", "ancestors": [], - "description": "Performs a series of actions: it reads and computes a graph from a text file, draws the graph using Matplotlib, and provides methods for navigating and querying the graph.", + "description": "Computes and draws a graph representation of a long-term memory space, allowing for door-based navigation and metric reconstruction. It takes in a set of rooms and their connections as well as a base room name and computes the metric map between them.", "attributes": [ { "name": "g", - "type_name": "undetected", - "description": "Used to store the graph data structure for computing the metric reconstruction." + "type_name": "Graph", + "description": "Used to store a graph representation of a long-term spatial memory task. It contains nodes, edges, and types (rooms, doors, walls) that are used to compute the metric map." }, { "name": "read_graph", - "type_name": "method", - "description": "Responsible for reading a graph from a file in Graphviz format. It takes no arguments and returns a Graph object." + "type_name": "instance", + "description": "Used to read a graph from a pickled file. It returns a `Subgraph` object representing the graph." }, { "name": "fig", "type_name": "matplotlibfigureFigure", - "description": "Used to represent the figure object for visualizing the graph." + "description": "Used to store the figure object that will be drawn with the graph." }, { "name": "ax", - "type_name": "PyQt5QtWidgetsQGraphicsScene", - "description": "Used to represent the graphical display area where the graph will be drawn." + "type_name": "Axes", + "description": "Used to store the axis object for plotting the graph." }, { "name": "fig_2", - "type_name": "matplotlibfigureFigure", - "description": "Used to store the figure object for the graph display." + "type_name": "MatplotlibFigure", + "description": "Used to store a figure object used for drawing the graph." }, { "name": "ax_2", - "type_name": "matplotlibFigure", - "description": "Used to store a second axis object for plotting the graph." + "type_name": "Axis", + "description": "Used to represent the graph's second axis, which is used for the metric reconstruction \nplot. It provides functions to set the title, labels, and limits for this axis." } ], "name": "LongTermGraph", @@ -51,20 +51,20 @@ "comment": null }, "item_type": "class", - "length": 230, + "length": 244, "docLength": null }, { - "id": "82d776a5-ee96-48ae-9e44-d4548fd1800e", + "id": "89c82564-d052-e2bf-6549-98b7754116ea", "ancestors": [ - "8aa24592-d2b2-699c-8b4a-818d3e97115b" + "7453d1a9-2093-7088-9941-e01ea3c40300" ], - "description": "Reads a graph from a file, creates two subplots in an matplotlib figure for visualizing the graph and its metric reconstruction, and sets up labels for the x- and y-axes. If the file is not found, it sets `self.g` to `None`.", + "description": "In the LongTermGraph class reads a graph from a file, creates subplots for metric reconstruction and LTSM display, and initializes the graph object.", "params": [ { "name": "file_name", "type_name": "str", - "description": "Used to specify the path of a graph file that contains a directed multigraph, which will be read by the function and processed for metric reconstruction." + "description": "Used to specify the name of the graph file to read." } ], "returns": null, @@ -81,23 +81,23 @@ "docLength": null }, { - "id": "61ef881c-4d99-fb93-ec44-afbc9d8a0b4d", + "id": "97a1ea39-6c18-2888-1d49-0d9df0c43f81", "ancestors": [ - "8aa24592-d2b2-699c-8b4a-818d3e97115b" + "7453d1a9-2093-7088-9941-e01ea3c40300" ], - "description": "Generates a graphical representation of a subgraph of a larger graph, based on node and edge attributes. It creates a scatter plot of room and door nodes, and plots edges as grey lines with node names annotated.", + "description": "Generates a graph visualization of a subgraph of a larger graph, based on node and edge types. It uses the `layout` parameter to specify the layout algorithm and colors nodes and edges based on their type.", "params": [ { "name": "only_rooms", "type_name": "bool", - "description": "Used to filter out nodes that are not rooms, doors, or walls." + "description": "Used to filter the nodes and edges displayed on the graph based on their types, with \"room\" being the default value for only rooms are shown." } ], "returns": null, "name": "draw_graph", "location": { - "start": 214, - "insert": 215, + "start": 220, + "insert": 221, "offset": " ", "indent": 8, "comment": null @@ -241,177 +241,187 @@ "path": "agents/g2o_agent/src/specificworker.py", "content": { "structured": { - "description": "A class named \"G2ORealtime\" that implements the G2O real-time algorithm for robot localization and mapping in a graphical representation. The code includes several methods for updating the graph, adding edges, and tracking the position of vertices over time. It also includes functions for updating node attributes and deleting nodes or edges. Overall, the code provides a robust framework for implementing the G2O real-time algorithm in a graphical environment.", + "description": "A class named `G2O` that handles various graph related tasks such as adding and deleting nodes and edges, updating edge attributes, and setting RT translation and rotation values. It also defines several functions for updating node attributes, updating edges, and deleting nodes or edges. The code uses the `networkx` library to handle graphs and their operations.\n\nIn summary, the code is a class that manages graph-related tasks for an RT (Real-Time) system, including adding and removing nodes and edges, updating edge attributes, and setting RT translation and rotation values.", "items": [ { - "id": "84c303b7-592e-bca1-e44c-174991e2df71", + "id": "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb", "ancestors": [], - "description": "Performs real-time visual odometry estimation for a robot using a G2O graph and RT communication. It initializes a graph, adds nominal corners, and updates edges with measured data from an RT communication agent.", + "description": "Is responsible for processing robot poses and creating a graph representation of the environment for a specific worker in a multi-robot system, including handling edges, vertices, odometry, and room changes.", "attributes": [ { "name": "Period", - "type_name": "float", - "description": "Used to control the interval at which the worker performs its tasks, with a default value of 1 second. It can be used to adjust the worker's execution speed." + "type_name": "instance", + "description": "Used to specify the time interval between updates of the worker's state, allowing for more efficient processing of large datasets." }, { "name": "agent_id", "type_name": "int", - "description": "A unique identifier assigned to each agent in the system." + "description": "Used as a identifier for the agent that owns the specific worker." }, { "name": "g", "type_name": "Graph", - "description": "Used for representing the graph of a robot's motion, where each node represents a pose of the robot and each edge represents a motion of the robot between two poses. The graph is used to compute the marginal probability distribution of the robot's motion given its initial position and velocity." + "description": "Used to store the graph representation of the environment, which is updated based on the worker's observations and actions." }, { "name": "startup_check", "type_name": "QTimersingleShot", - "description": "Used to start a quit timer after 200 milliseconds of inactivity, which allows the worker to exit gracefully when no further work is available." + "description": "Used to check if the application should quit after a certain time has elapsed since startup. It's used to implement the main loop of the worker, where the worker periodically checks if it should quit based on the current time." }, { "name": "rt_api", - "type_name": "str", - "description": "8 characters long. It stores a string value that represents the ROS launch file path for the RT node." + "type_name": "instance", + "description": "Used to store the RT API object, which is used for real-time data processing and transmission." }, { "name": "inner_api", "type_name": "instance", - "description": "Used as a reference to the inner API object, which is responsible for handling the actual work of the worker." + "description": "Used to store the inner API object of the worker, which allows for communication between the worker and the main thread." }, { "name": "odometry_node_id", "type_name": "int", - "description": "0-based index of the node representing the robot's odometry information in the graph. It is used to identify the node in the graph that represents the robot's current position and velocity." + "description": "3 in this example, indicating that the node with ID 3 represents the odometry information." }, { "name": "odometry_queue", "type_name": "3D", - "description": "A queue that stores the odometry data of the robot, which is used to update the graph and calculate the robot's pose." + "description": "A list of tuples containing the current pose of the robot, which is updated at each step by the worker. The queue stores the last `N` poses for each node in the graph, where `N` is a user-defined parameter." }, { "name": "last_odometry", "type_name": "3D", - "description": "Used to store the last known odometry value of the robot, which is used for calculating the displacement and covariance matrix." + "description": "Used to store the last known odometry of the robot, which can be used for visualization and debugging purposes." }, { "name": "g2o", "type_name": "3D", - "description": "A G2O graph, which is a data structure used to represent the robot's pose in a 3D environment. It stores the information of the robot's pose in a set of nodes and edges, where each node represents a location in space and each edge represents a connection between two locations. The `g2o` attribute is used to perform optimization tasks such as finding the robot's pose that minimizes a cost function." + "description": "Used to represent the pose (position, orientation, and scale) of a robot or other object in 3D space. It is used in conjunction with the `get_covariance_matrix` method to compute the covariance matrix of the robot's position and orientation." + }, + { + "name": "visualizer", + "type_name": "instance", + "description": "A reference to an instance of the `Visualizer` class, which provides a user interface for visualizing the robot's pose and other relevant data in real-time." }, { "name": "odometry_noise_std_dev", "type_name": "float", - "description": "Used to represent the standard deviation of odometry noise in the worker's graph. It is used in the `get_displacement` method to compute the displacement between two poses based on the odometry noise." + "description": "0.1 by default, which represents the standard deviation of the noise added to the odometry values during the simulation for more realistic results." }, { "name": "odometry_noise_angle_std_dev", - "type_name": "floatingpoint", - "description": "1.5 by default, which represents the standard deviation of angle noise in the odometry measurement of a robot. It affects the accuracy of the robot's positioning and movement tracking." + "type_name": "float", + "description": "0.8 by default, which represents the standard deviation of the angular noise in the odometry measurement. It affects the optimization process by controlling the spread of the angle values in the graph." }, { "name": "measurement_noise_std_dev", "type_name": "float", - "description": "10% of the robot's minimum distance from its reference configuration to its target position, indicating the standard deviation of measurement noise in the robot's pose estimation." + "description": "Used to represent the standard deviation of noise in the robot's measurements. It is used in the worker's implementation to scale the measurement values to account for the noise in the data." }, { "name": "last_room_id", "type_name": "int", - "description": "Used to store the previous room ID that was visited by the agent before the current room. It is updated when the agent moves to a new room, allowing the worker to detect changes in the environment." + "description": "Used to store the last room ID seen before the agent switched rooms, which is used to handle room changes during navigation tasks." }, { "name": "actual_room_id", "type_name": "int", - "description": "Used to store the current room ID of the agent, which is updated when the robot moves from one room to another." + "description": "Used to keep track of the current room id that the worker is in, it's updated when the worker moves to a new room." }, { "name": "elapsed", "type_name": "int", - "description": "Used to store the time elapsed since the last call to the `startup_check` function, which is called every 0.1 seconds. It is used to determine when to quit the application." + "description": "Used to track the elapsed time since the worker was created, which can be used to control the worker's execution time and prevent it from running for too long." }, { "name": "room_initialized", "type_name": "bool", - "description": "Used to keep track of whether the current room has been initialized or not. It's set to False when a new room is entered, and True otherwise." + "description": "Used to track whether the robot's room has been initialized or not. It is set to False when the robot moves to a new room, and True otherwise." }, { "name": "iterations", "type_name": "int", - "description": "Used to keep track of the number of iterations performed by the worker. It increases by one each time a new iteration is performed and can be accessed or modified from within the worker's methods." + "description": "Used to count the number of iterations performed by the worker during its execution. It is incremented each time the worker processes a new iteration of the graph optimization problem." }, { "name": "hide", "type_name": "str", - "description": "Used to specify whether a worker should be hidden or not when multiple workers are running simultaneously." + "description": "Used to indicate whether the worker should be hidden or not, defaulting to False." }, { "name": "init_graph", "type_name": "bool", - "description": "Used to indicate whether the graph has been initialized or not. It is set to True when the graph is initialized and False otherwise, allowing the worker to distinguish between initialized and uninitialized graphs." + "description": "Used to keep track of whether the graph has been initialized or not, during the processing of the ROS bag file. It's set to True when the graph is first constructed and False when the graph is reconstructed after a change in the environment." }, { "name": "current_edge_set", "type_name": "bool", - "description": "Used to indicate whether the current edge set has been computed or not. It is set to true when the edge set is first computed and false otherwise, so that only new computations are performed." + "description": "Used to indicate whether the current edge set has been updated or not. It's used to track the updates of the edges in real-time." }, { "name": "first_rt_set", "type_name": "bool", - "description": "Set to `True` when a new RT translation and rotation are detected, and `False` otherwise. It indicates whether the worker has received its first RT set or not." + "description": "Set to `True` when the worker receives its first RT (Real-time) edge from a \"room\" node, indicating that the worker should start tracking the RT translation and rotation for this room." }, { "name": "translation_to_set", "type_name": "3D", - "description": "Used to store the translation from the robot's reference frame to the set frame." + "description": "Used to store the translation of a robot to a set point, calculated based on the RT messages it receives. It is updated in the `update_edge_att` method." }, { "name": "rotation_to_set", "type_name": "3D", - "description": "Used to store the rotation of a robot's end effector relative to its base frame, which is necessary for setting the desired orientation of the robot's end effector." + "description": "Used to store the rotation of the robot's end effector with respect to a set reference frame. It is updated whenever the robot's pose changes, and is used in the computation of the robot's covariance matrix." }, { "name": "room_polygon", - "type_name": "numpyarray", - "description": "Used to store the coordinates of a room's boundary in a list of (x,y) tuples." + "type_name": "Polygon", + "description": "Used to store the polygon representation of a room in the environment. It is used for collision detection and other purposes related to navigation and mapping." + }, + { + "name": "security_polygon", + "type_name": "3D", + "description": "Used to store the security polygon of the robot, which is a convex polytope used for collision detection and avoidance." }, { "name": "initialize_g2o_graph", "type_name": "instance", - "description": "Used to initialize a Graph2ONode graph for a specific robotic arm. It creates nodes, edges, and defines the room node, and sets up the optimization algorithm and visualization tools." + "description": "Used to initialize the graph2onedges (G2O) graph during the initialization of the worker, allowing for faster computation of marginals in the optimize method." }, { "name": "rt_set_last_time", - "type_name": "float", - "description": "Used to store the last time when a RT set was set by the worker, which is used to filter out unnecessary RT sets." + "type_name": "int", + "description": "Used to store the time when the first RT set was observed by the worker, for updating the translation and rotation of the robot to the set." }, { "name": "rt_time_min", "type_name": "float", - "description": "Set to the minimum time between two consecutive RT sets. It is used to determine when to reset the translation and rotation values for RT tracking purposes." + "description": "Defined as `self.rt_time_min = 10`. It represents the minimum time interval between two RT sets for the robot to be considered in the RT algorithm." }, { "name": "timer", "type_name": "QTimer", - "description": "Used to schedule periodic updates of the graph with the current robot state. It is set to single shot every 200 milliseconds, allowing for real-time updates of the graph." + "description": "Used to create a timer that triggers the worker's method to update the graph every 200 milliseconds." }, { "name": "compute", - "type_name": "instance", - "description": "Used to compute the marginal probability distributions of the robot's state given its current observations. It takes an argument `vertex` which is a vertex in the graph representing the robot's state, and returns a tuple containing the marginal probabilities and the Hessian matrix of the robot's state." + "type_name": "lambda", + "description": "Called when a task is assigned to the worker. It takes the task as input, processes it using a provided function, and returns the result as output." }, { "name": "update_node_att", - "type_name": "update", - "description": "Called when a new node with the specified id is added to the graph, or an existing node's attributes are updated. It updates the odometry queue based on the new node's attributes and sets the room initialization flag accordingly." + "type_name": "Python", + "description": "Used to update the attributes of a node in the graph when its ID matches the specified ID." }, { "name": "update_edge", - "type_name": "edge", - "description": "Used to update the attributes of an edge in a graph when the edge's node types change or when a new edge is added to the graph. It sets the `rt_translation` and `rotation_euler_xyz` attributes of the edge based on the new node types." + "type_name": "update", + "description": "Called whenever an edge's attributes change. It checks if the edge is a \"room\" edge and updates the \n\"translation to set\", \"rotation to set\", or \"first RT set\" variables based on the edge's type and \nattributes." }, { "name": "update_edge_att", - "type_name": "attribute", - "description": "Used to update the attributes of an edge in a graph. It takes the edge ID, the new value for the attribute, and the name of the attribute as input parameters." + "type_name": "edge", + "description": "Used to update the attributes of an edge in a graph." } ], "name": "SpecificWorker", @@ -423,25 +433,25 @@ "comment": null }, "item_type": "class", - "length": 302, + "length": 404, "docLength": null }, { - "id": "25b76ae0-c0dd-c6a7-fc4e-5e6acf507c0a", + "id": "7d7de271-5f36-6685-a041-992c749eeb89", "ancestors": [ - "84c303b7-592e-bca1-e44c-174991e2df71" + "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" ], - "description": "Initializes a SpecificWorker instance, defining its attributes and connections to graphs, timers, and signal handlers.", + "description": "Initializes member variables and sets up event handling for signals related to the graph, nodes, edges, and attributes.", "params": [ { "name": "proxy_map", "type_name": "dict", - "description": "Used to store a mapping from agent IDs to graphs." + "description": "Used to map agent id's to their corresponding DSRGraph objects." }, { "name": "startup_check", - "type_name": "Optionalbool", - "description": "Used to check if the robot's graph has already been initialized before starting the main loop." + "type_name": "bool", + "description": "Used to determine whether to run the `startup_check` method or not when initializing the SpecificWorker object." } ], "returns": null, @@ -454,73 +464,73 @@ "comment": null }, "item_type": "method", - "length": 51, + "length": 54, "docLength": null }, { - "id": "1223cd80-b422-d1a2-544e-d32d51f481f3", + "id": "da0a9261-8bf8-ea87-794b-f1499a4f53d3", "ancestors": [ - "84c303b7-592e-bca1-e44c-174991e2df71" + "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" ], - "description": "Generates high-quality summaries of Java code that is given to it. It takes into account the robot's odometry, computes the translation and rotation of the robot relative to a reference frame, and adds the information to the graph. It also updates the landmark in the graph.", + "description": "Updates the robot's position, orientations, and security polygon based on the odometry data and the RT graph.", "params": [], "returns": null, "name": "compute", "location": { - "start": 120, - "insert": 122, + "start": 121, + "insert": 123, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 71, + "length": 90, "docLength": null }, { - "id": "3fbfed7d-c225-7ead-9a4a-90ba5ee3ffd1", + "id": "8a63dc4d-4b58-b681-ff4d-4e5147446630", "ancestors": [ - "84c303b7-592e-bca1-e44c-174991e2df71" + "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" ], - "description": "Initializes a Graph2O graph for a specific robot and room, adding nominal corners and measuring them to create a polygonal representation of the room. It also sets the robot's initial position and orientation in the graph.", + "description": "Initializes a Graph-based Object Recognition (G2O) graph for a specific room by:\n\n1. Finding the corners of the room using ROS topics.\n2. Adding nominal corners to the G2O graph.\n3. Adding fixed poses to the G2O graph for the robot and doors in the room.", "params": [], "returns": { "type_name": "bool", - "description": "1 if the g2o graph was successfully initialized with the given room id, and 0 otherwise." + "description": "1 if the initialization of the g2o graph was successful, and 0 otherwise." }, "name": "initialize_g2o_graph", "location": { - "start": 214, - "insert": 215, + "start": 238, + "insert": 241, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 62, + "length": 109, "docLength": null }, { - "id": "253cc8ea-c52a-78a0-8844-da420c7a91fe", + "id": "bef4919b-9b49-2ea6-7e42-9773a84c6c4e", "ancestors": [ - "84c303b7-592e-bca1-e44c-174991e2df71" + "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" ], - "description": "Calculates the displacement of a robot based on its odometry data, updating the robot's position and orientation using the lateral, angular, and linear displacements.", + "description": "Calculates the displacement of an object based on its odometry data, computing the lateral displacement, forward displacement, and angular displacement using a moving window approach.", "params": [ { "name": "odometry", - "type_name": "stdvectordouble", - "description": "3D vector representing the robot's position, orientation, and velocity at a given time stamp." + "type_name": "3element", + "description": "Used to store the robot's motion information at each time step, including its linear displacement, lateral displacement, and angular displacement." } ], "returns": { - "type_name": "triplet", - "description": "3 elements long (desplazamiento lateral, displacement avance and angular displacement)." + "type_name": "3element", + "description": "Displacement in three directions (lateral, advance and angular) calculated using the odometry data from a queue." }, "name": "get_displacement", "location": { - "start": 285, - "insert": 286, + "start": 437, + "insert": 438, "offset": " ", "indent": 8, "comment": null @@ -530,26 +540,26 @@ "docLength": null }, { - "id": "35fb82ad-3303-53a5-4b40-b15c8f361fa3", + "id": "ae20cbb0-b446-7387-2246-a1505aa2b1c1", "ancestors": [ - "84c303b7-592e-bca1-e44c-174991e2df71" + "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" ], - "description": "Computes the covariance matrix of a set of vertices in a graph using the G2O optimization algorithm. It takes a vertex as input, returns the computed covariance matrix and a result flag indicating whether the computation was successful.", + "description": "Computes the covariance matrix for a given vertex in a graph, using an optimization algorithm to compute the marginals of the vertices and then constructing the covariance matrix.", "params": [ { "name": "vertex", - "type_name": "g2oVertex", - "description": "Passed to compute marginals for computing covariance matrix." + "type_name": "g2overtex", + "description": "Used to compute the covariance matrix of the vertices in the graph." } ], "returns": { "type_name": "tuple", - "description": "2-element, containing the result of computing covariance matrix and the marginals of the vertices." + "description": "2-element, where the first element is a boolean value indicating whether the covariance matrix was computed or not and the second element is the actual covariance matrix." }, "name": "get_covariance_matrix", "location": { - "start": 304, - "insert": 305, + "start": 456, + "insert": 457, "offset": " ", "indent": 8, "comment": null @@ -559,23 +569,23 @@ "docLength": null }, { - "id": "16329532-8cfc-7083-4b42-b215340de3c5", + "id": "67619954-9d16-ceb2-ee47-9e0c50ce26bd", "ancestors": [ - "84c303b7-592e-bca1-e44c-174991e2df71" + "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" ], - "description": "Visualizes a G2O optimization problem in real-time, loading the problem from an file and plotting the vertices and edges as 3D scatter plots while updating the display with small intervals of time.", + "description": "Within the `SpecificWorker` class loads an G2O file, visualizes its vertices and edges in 3D, and updates the visualization in real-time as the optimizer processes new measurements.", "params": [ { "name": "optimizer", "type_name": "instance", - "description": "Used to load G2O files for visualization in real-time." + "description": "Used to load G2O files." } ], "returns": null, "name": "visualize_g2o_realtime", "location": { - "start": 317, - "insert": 318, + "start": 469, + "insert": 470, "offset": " ", "indent": 8, "comment": null @@ -585,28 +595,28 @@ "docLength": null }, { - "id": "06023ea6-5dc9-1c87-3541-4399e058c863", + "id": "ef528e02-66ad-7da9-964d-91a42afb8717", "ancestors": [ - "84c303b7-592e-bca1-e44c-174991e2df71" + "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" ], - "description": "Updates the attributes of a node in a graph, specifically the `odometry_queue` attribute, when the node's ID matches the `odometry_node_id`.", + "description": "Updates the attributes of a node in a graph, specifically the odometry node, by appending values to an odometry queue.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to identify the node for which attribute values are being updated." + "description": "Used to represent the node ID of interest for updating attributes." }, { "name": "attribute_names", "type_name": "[str]", - "description": "A list of attribute names to update in the node." + "description": "An array of strings representing the names of attributes to be updated on the node." } ], "returns": null, "name": "update_node_att", "location": { - "start": 354, - "insert": 363, + "start": 506, + "insert": 515, "offset": " ", "indent": 8, "comment": null @@ -616,11 +626,11 @@ "docLength": null }, { - "id": "fcfa7684-299e-0cbf-024c-e68fe02c0453", + "id": "fa72d1a2-e9a6-1481-7d44-8a7f242085b0", "ancestors": [ - "84c303b7-592e-bca1-e44c-174991e2df71" + "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" ], - "description": "Updates the node with the specified ID, based on the type of node it represents.", + "description": "Updates an unknown node with the specified ID, performing different actions based on the type of node it represents.", "params": [ { "name": "id", @@ -630,14 +640,14 @@ { "name": "type", "type_name": "str", - "description": "Used to specify the node type." + "description": "Used to specify the node's type, with possible values being \"corner\"." } ], "returns": null, "name": "update_node", "location": { - "start": 370, - "insert": 380, + "start": 522, + "insert": 532, "offset": " ", "indent": 8, "comment": null @@ -647,11 +657,11 @@ "docLength": null }, { - "id": "fced3417-1e29-05b8-ce46-5f86e6d4eff4", + "id": "3870a905-4bed-33bc-1444-26249501564e", "ancestors": [ - "84c303b7-592e-bca1-e44c-174991e2df71" + "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" ], - "description": "Deletes a node from the graph represented by the `SpecificWorker` class.", + "description": "Deletes a node from the graph represented by the `SpecificWorker` class, setting the `room_initialized` attribute to `False`.", "params": [ { "name": "id", @@ -662,8 +672,8 @@ "returns": null, "name": "delete_node", "location": { - "start": 382, - "insert": 383, + "start": 534, + "insert": 535, "offset": " ", "indent": 8, "comment": null @@ -673,33 +683,33 @@ "docLength": null }, { - "id": "8392cc88-c41a-3d84-7741-474d984fa20d", + "id": "21b579b0-e2c1-b8af-0d49-f97a5f6121e1", "ancestors": [ - "84c303b7-592e-bca1-e44c-174991e2df71" + "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" ], - "description": "Updates the room ID and sets the current edge set based on the provided edge type and node types.", + "description": "Updates the room ID and sets the current edge set to true when the type is \"current\" or \"RT\" and the source node is a room and the target node is \"Shadow\". It also sets the translation and rotation to set variables when RT is set for the first time.", "params": [ { "name": "fr", "type_name": "int", - "description": "Representative of the start node of an edge in a graph." + "description": "Used to represent the ID of the current node being processed in the graph." }, { "name": "to", "type_name": "int", - "description": "Used to represent the target node of an edge in the graph." + "description": "The id of the next node in the graph that the edge is pointing to." }, { "name": "type", "type_name": "str", - "description": "Used to specify the type of edge to update, either \"current\" or \"RT\"." + "description": "Used to determine which room the edge updates are for, either the current room or RT (room to Shadow)" } ], "returns": null, "name": "update_edge", "location": { - "start": 391, - "insert": 392, + "start": 543, + "insert": 544, "offset": " ", "indent": 8, "comment": null @@ -709,33 +719,33 @@ "docLength": null }, { - "id": "644bd490-10bb-9e96-d442-ff5ba8edb07d", + "id": "5a1c2562-2abe-0a91-2f4d-cda682735284", "ancestors": [ - "84c303b7-592e-bca1-e44c-174991e2df71" + "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" ], - "description": "Deletes an edge from a graph, specified by the vertex indices `fr` and `to`, and the edge type.", + "description": "Deletes an edge from a graph with specified source and target nodes, based on the type of edge.", "params": [ { "name": "fr", "type_name": "int", - "description": "Used to represent the starting vertex index of an edge to be deleted." + "description": "Given a value of 13." }, { "name": "to", "type_name": "int", - "description": "Used to indicate the destination node ID for edge deletion." + "description": "Specified as an index of an edge to be deleted." }, { "name": "type", "type_name": "str", - "description": "Used to specify the edge type to be deleted, with possible values 'in' or 'out'." + "description": "Used to specify the edge type to be deleted." } ], "returns": null, "name": "delete_edge", "location": { - "start": 419, - "insert": 420, + "start": 571, + "insert": 572, "offset": " ", "indent": 8, "comment": null @@ -1148,132 +1158,137 @@ "path": "agents/long_term_spatial_memory_agent/src/specificworker.py", "content": { "structured": { - "description": "An autonomous robot navigation agent using Graphical User Interface (GUI) for human interaction and a graph (G) to represent the environment. The code performs various tasks such as:\n\n* Reading from a json file containing the environment data and initializing the GUI window\n* Drawing a room image and displaying it on the GUI\n* Creating edges and nodes in the graph representing the environment, robot, and doors\n* Updating node attributes based on user input\n* Inserting current edge to the room\n* Starting the robot navigation process\n\nThe code uses various packages such as `numpy`, `cv2`, `QTimer`, and `QApplication` for data manipulation, image processing, and GUI functionality.", + "description": "An agent's internal graph and performs various operations on it, including:\n\n* Generating a room picture by iterating over the room nodes and their RT edges, and drawing a black image proportional to the room size with the room polygon and doors circled in white.\n* Inserting current edge to the room when there is no current edge and the agent's state is \"crossed\".\n* Updating node attributes and node type based on user input.\n* Deleting nodes and edges based on user input.", "items": [ { - "id": "76fa4ef8-3f83-f299-c341-308421fee7c7", + "id": "0a067d35-4dda-36b4-5c41-7f6586689420", "ancestors": [], - "description": "Is responsible for processing specific types of nodes and edges in a graph, such as room nodes and their corresponding doors, as well as affordance nodes and their transitions. It also handles updating the graph's attributes and inserting new edges.", + "description": "Performs various tasks related to a specific worker in a multi-agent system, such as checking for room occupation, updating node attributes, and generating room images. It also handles insertion and deletion of edges and nodes, and updates the state of the agent based on certain conditions.", "attributes": [ { "name": "Period", - "type_name": "float32", - "description": "0 by default. It represents the period of time taken for a specific worker to complete its task, which can be used to determine the next task to assign to the worker based on its current state." + "type_name": "int", + "description": "200 milliseconds, which is the time interval between updates of the worker's state." }, { "name": "agent_id", - "type_name": "int", - "description": "0 by default, representing the agent's internal ID for distinguishing it from other agents." + "type_name": "integerint", + "description": "Used as a unique identifier for each agent in the swarm." }, { "name": "g", "type_name": "Graph", - "description": "Used to store the graph data structure for the environment map navigation. It contains the nodes, edges, and other attributes needed for the worker's functionality." + "description": "Used to store the graph representation of the environment that the worker is working on. It contains nodes, edges, and other attributes that are relevant for the worker's task." }, { "name": "update_node", - "type_name": "attribute", - "description": "Called when a node's attributes change. It updates the state of the worker based on the new attributes." + "type_name": "UpdateNode", + "description": "Used to update the state of a node in the graph based on its ID, type, and affordance state." }, { "name": "update_edge", - "type_name": "Edge", - "description": "Used to update the edge attributes based on the robot's movement. It takes three parameters: `fr`, `to`, and `type`, which represent the from, to, and edge type respectively. The method updates the edge attributes accordingly and sets the current edge as \"current\" if there are no existing \"current\" edges." + "type_name": "str", + "description": "Used to set a new current edge in the graph when there is no existing current edge and the room node exists." }, { "name": "startup_check", "type_name": "QTimersingleShot", - "description": "Used to schedule the worker's main loop to run after 200 milliseconds of starting the application." + "description": "Used to start the application's shutdown process after a certain period of time (200 milliseconds) since the worker was created." }, { "name": "rt_api", - "type_name": "Attribute", - "description": "Used to store the RT API of the worker, which is needed for communication between the worker and the main agent." + "type_name": "str", + "description": "Used for handling robot actions, specifically RT (Remote Transport) actions. It provides a way to interact with the robot's internal API to perform actions such as moving or manipulating objects." }, { "name": "inner_api", - "type_name": "internal", - "description": "Used by the worker to interact with its inner agent API, which handles the actual robot control and sensor data processing." + "type_name": "API", + "description": "Used to access the internal API of the agent, allowing the worker to interact with the agent's internal state and perform actions such as generating room pictures or updating node attributes." }, { "name": "robot_name", "type_name": "str", - "description": "Used to store the name of the robot that the worker is controlling." + "description": "Used to store the name of the robot agent in the environment." }, { "name": "robot_id", "type_name": "int", - "description": "Used to store the ID of the robot that the worker represents." + "description": "Used to store the ID of the robot node in the graph. It is used to identify the current room and for RT edges." }, { "name": "last_robot_pose", - "type_name": "Attribute", - "description": "Used to store the last known pose of the robot in the environment, which can be used for navigation and task completion purposes." + "type_name": "Pose", + "description": "Used to store the last known pose of the robot in the environment, which is used for motion planning and collision detection." + }, + { + "name": "robot_exit_pose", + "type_name": "Pose", + "description": "Used to store the pose of the robot when it exits a room." }, { "name": "state", "type_name": "str", - "description": "Used to keep track of the worker's current state, either \"idle\", \"running\", or \"crossed\"." + "description": "Used to store the current state of the worker, which can be either \"idle\", \"running\", or \"crossed\"." }, { "name": "affordance_node_active_id", "type_name": "int", - "description": "Used to store the ID of the affordance node that is currently active, indicating which affordance the worker should perform." + "description": "Used to store the ID of the node representing the affordance that the robot is currently interacting with." }, { "name": "exit_door_id", "type_name": "int", - "description": "0-based, representing the index of the door through which the robot exits a room when it moves from that room to another." + "description": "0 by default, indicating the door through which the robot exits a room when it moves to another room." }, { "name": "room_exit_door_id", "type_name": "int", - "description": "14 by default. It represents the ID of the door through which the robot exits a room." + "description": "31 by default, representing the door ID of the room that the worker is exiting." }, { "name": "enter_room_node_id", "type_name": "int", - "description": "Used to store the ID of the room node that the robot enters when it moves from the previous room to the current room." + "description": "Used to store the ID of the room node that the worker has entered. It is used to check if the worker has reached the goal state by comparing it with the ID of the goal room node." }, { "name": "graph", "type_name": "Graph", - "description": "Used to store the graph representation of the environment, including nodes, edges, and their attributes." + "description": "Used to represent the graph of nodes and edges in the environment, which is updated based on the worker's actions and observations." }, { "name": "vertex_size", "type_name": "int", - "description": "Used to represent the size of a vertex in a graph, indicating its importance or relevance in the graph structure." + "description": "16 by default, indicating the size of each vertex in the graph." }, { "name": "not_required_attrs", "type_name": "list", - "description": "Used to store the names of attributes that are not required for the worker's functionality, i.e., they are optional or not used in the implementation." + "description": "Used to store a list of node attributes that are not required for the worker's functionality. It helps in avoiding unnecessary computation and improving performance by skipping the retrieval of these attributes during node traversal." }, { "name": "long_term_graph", "type_name": "Graph", - "description": "Used to store the long-term graph of the agent, which is updated at each iteration of the worker. It keeps track of the agent's internal graph over time, allowing the worker to perform actions based on the historical information of the environment." + "description": "Used to store the long-term graph of the environment, which represents the entire state of the environment over time." }, { "name": "global_map", - "type_name": "ndarray", - "description": "2D, representing a map of the environment. It stores the occupancy probabilities of each cell in the grid, which are used to calculate the worker's attention and motion." + "type_name": "npndarray", + "description": "Used to store a map of the environment, which is used for navigation and other purposes." }, { "name": "insert_current_edge", "type_name": "Edge", - "description": "Used to insert a new edge into the graph with a specified type (current) and source and destination nodes, signaling that the current room has been reached and the agent should move on to the next task." + "description": "Used to insert a new edge into the graph with the current room as the source node and the robot node as the destination node, indicating that the robot has entered the current room." }, { "name": "timer", "type_name": "QTimer", - "description": "Used to schedule the worker's startup check after a delay of 200 milliseconds." + "description": "Used to schedule a call to the `startup_check()` function after 200 milliseconds of starting the application, which signals the agent to start its work." }, { "name": "compute", "type_name": "instance", - "description": "Used to compute the next action for the agent based on the current state of the environment. It takes into account the agent's attributes, such as its position and orientation, and returns a dict with the next action as key and the corresponding value as value." + "description": "Used to compute the cost of the robot moving from its current state to a new state. It takes into account factors such as distance, obstacles, and door states." } ], "name": "SpecificWorker", @@ -1285,25 +1300,25 @@ "comment": null }, "item_type": "class", - "length": 508, + "length": 533, "docLength": null }, { - "id": "30924e5c-a8d3-aabb-bf47-aa7f060f80a2", + "id": "8f7fe684-295f-f8b1-c94d-1c5ce453de90", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Sets up the internal structure and event connections for a SpecificWorker instance, including its graph, node ID, and timer for computing updates.", + "description": "Initializes an instance of the SpecificWorker class by setting up various graph structures, defining variables, and connecting signals for updating node and edge positions.", "params": [ { "name": "proxy_map", "type_name": "igraphGraph", - "description": "Used to specify the graph for this specific worker instance." + "description": "Used to specify the graph that represents the environment for the worker agent." }, { "name": "startup_check", "type_name": "bool", - "description": "Used to check if any of the required nodes are present in the graph before starting the agent's inner loop." + "description": "Used to check if the agent has already started. If set to `False`, it will call the `startup_check` method instead, which initializes the internal state of the agent." } ], "returns": null, @@ -1316,20 +1331,20 @@ "comment": null }, "item_type": "method", - "length": 45, + "length": 46, "docLength": null }, { - "id": "9d1d8a47-e09e-bba1-ea44-c61dcc5ed6f9", + "id": "b7663dc2-9b1f-eaaf-a64b-a95a80099f99", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Sets parameters and updates node attributes in a graph. It removes a self-edge from a room, stores an exit door ID, and assigns other_side_door attributes to both doors leading to the new room.", + "description": "Sets parameters and modifies the state of a Room object by removing a self-edge, storing an exit door ID, setting attribute values for doors, and reading an entrance door node to add an other_side_door attribute with the name of the exit door in the new room.", "params": [ { "name": "params", "type_name": "object", - "description": "Passed to the function for configuration purposes." + "description": "Passed in from outside the function to be processed or modified." } ], "returns": { @@ -1338,8 +1353,8 @@ }, "name": "setParams", "location": { - "start": 115, - "insert": 116, + "start": 116, + "insert": 117, "offset": " ", "indent": 8, "comment": null @@ -1349,17 +1364,17 @@ "docLength": null }, { - "id": "6d2c558e-51df-ebbf-9b45-c373d8e16ac8", + "id": "4b952eb9-acba-209b-de45-48fe3b8d2de5", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Performs various actions based on the current state of the worker, including idle, crossing, crossed, initializing room, known room, initializing doors, storing graph, and removing.", + "description": "Determines the state of a worker based on the current state of the graph and performs appropriate actions according to the state.", "params": [], "returns": null, "name": "compute", "location": { - "start": 129, - "insert": 148, + "start": 130, + "insert": 149, "offset": " ", "indent": 8, "comment": null @@ -1369,37 +1384,37 @@ "docLength": null }, { - "id": "00a4e420-1c1d-efa6-f340-28f5c78e4e52", + "id": "59d68e36-a969-1390-a64b-66a3c5f1207a", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Determines if the agent is entering or exiting a room based on the affordance graph and updates the agent's state and door associations accordingly.", + "description": "Generates high-quality summaries of Java code given to it, checks if any \"current\" edge exists, and if not, associates doors based on their proximity.", "params": [], "returns": null, "name": "idle", "location": { - "start": 167, - "insert": 169, + "start": 168, + "insert": 170, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 64, + "length": 71, "docLength": null }, { - "id": "bd4ee7e6-5efa-6487-9e4f-53d59567192c", + "id": "73ce6331-230e-b191-4445-a4e68e4cfb3a", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Determines if the agent has reached a door and, if so, updates its state based on the door's properties.", + "description": "Manages the node and edge properties for a worker class called `SpecificWorker`, which is an extension of `GenericWorker`. It retrieves the affordance node, checks if it has a parent, and updates the exit door ID node with the connected room name.", "params": [], "returns": null, "name": "crossed", "location": { - "start": 258, - "insert": 260, + "start": 264, + "insert": 266, "offset": " ", "indent": 8, "comment": null @@ -1409,17 +1424,17 @@ "docLength": null }, { - "id": "cc00d913-dfa4-edab-9447-2568f6c1e7d6", + "id": "b038074f-0d1d-37bb-7949-1863b4f1a32a", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Determines and stores the ID of the room node to enter when it is called, and updates the state of the worker to \"initializing doors\".", + "description": "Determines if there are any rooms other than the current room that the worker is in, and if so, sets the enter room node ID and inserts an edge to it. It then sets the state to \"initializing doors\" and prints a message.", "params": [], "returns": null, "name": "initializing_room", "location": { - "start": 278, - "insert": 281, + "start": 284, + "insert": 287, "offset": " ", "indent": 8, "comment": null @@ -1429,37 +1444,37 @@ "docLength": null }, { - "id": "53f4c0b8-56dd-07b3-0440-be3e6683e8df", + "id": "40ac65e2-76b8-2b98-194e-80c545d8fd7a", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Performs a series of actions to handle a robot's movement through a door, including:\n\n1. Finding the other side room node in the graph.\n2. Creating a new edge and vertex for the robot's new location.\n3. Deleting the original edge connecting the robot to the room.", + "description": "In the `SpecificWorker` class is responsible for handling the scenario where the robot needs to move to another room through a door, and there is no direct path from the current room to the desired room. It traverses the graph to find the shortest path to the desired room, updates the node's parent attribute, and inserts a new edge in the graph to connect the robot to the desired room.", "params": [], "returns": null, "name": "known_room", "location": { - "start": 292, - "insert": 294, + "start": 298, + "insert": 300, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 56, + "length": 73, "docLength": null }, { - "id": "2fab6259-032f-8abf-4744-db2e9c5a2fe7", + "id": "97fb42e6-e393-a18f-0d40-f962134e20f1", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "1) retrieves exit edges and matching same-side doors, 2) sets other side door name and connected room name attributes for each exit door node, and 3) associates doors between nodes using their names.", + "description": "Identifies the doors connected to the current room and updates their attributes with the names of the rooms they lead to, also associating them through a pair of tuples.", "params": [], "returns": null, "name": "initializing_doors", "location": { - "start": 362, - "insert": 364, + "start": 392, + "insert": 394, "offset": " ", "indent": 8, "comment": null @@ -1469,28 +1484,28 @@ "docLength": null }, { - "id": "50fd66d9-17e8-659a-6941-9c2265014ec1", + "id": "169b50a4-d8c6-22be-b548-e904c2d484da", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Takes two door nodes as input and connects them by adding an edge between them, while also updating the \"other side door name\" and \"connected room name\" attributes of each node to reflect the connection.", + "description": "Connects two doors in a graph by adding an edge between their nodes and updating the \"other side door name\" and \"connected room name\" attributes of each door node.", "params": [ { "name": "door_1", - "type_name": "str", - "description": "Represented as an tuple containing the name of the door and its room, such as (\"Door 1\", \"Room 1\")." + "type_name": "igraphNode", + "description": "A reference to one of the doors in the graph, which is used to establish a connection between two rooms." }, { "name": "door_2", "type_name": "str", - "description": "Represented as a string containing the name of the second door to be associated with the graph." + "description": "2-dimensional list containing the name of the door and its room information." } ], "returns": null, "name": "associate_doors", "location": { - "start": 401, - "insert": 403, + "start": 431, + "insert": 433, "offset": " ", "indent": 8, "comment": null @@ -1500,17 +1515,17 @@ "docLength": null }, { - "id": "a63ad4a9-7162-9788-6f48-a0ea5ee1a361", + "id": "7dba6d19-0ac9-11a8-5f4d-7ab151351297", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Stores the graph represented by an instance of `GenericWorker` in a file named \"graph.pkl\".", + "description": "Stores the graph data in a file called \"graph.pkl\" when a room node is found in the igraph data structure.", "params": [], "returns": null, "name": "store_graph", "location": { - "start": 420, - "insert": 421, + "start": 450, + "insert": 451, "offset": " ", "indent": 8, "comment": null @@ -1520,17 +1535,17 @@ "docLength": null }, { - "id": "72e8cae2-7f84-98bc-4243-263cb1655204", + "id": "6003f927-ac68-b2ba-564e-ba42175519a0", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Removes edges from the graph that connect two nodes with the same room number, and also deletes nodes that have only one connection to another node with a room number of 200 (shadow nodes). The function then updates the long-term graph and sets the state to \"idle\".", + "description": "Removes edges and nodes from the graph based on certain room numbers, updates the long-term graph, and changes the state to \"idle\".", "params": [], "returns": null, "name": "removing", "location": { - "start": 434, - "insert": 436, + "start": 464, + "insert": 466, "offset": " ", "indent": 8, "comment": null @@ -1540,23 +1555,23 @@ "docLength": null }, { - "id": "64fe063f-2993-c6bc-aa4d-981c1f3a4e37", + "id": "1ab47154-ba0f-2487-4144-57a467c74f40", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Iterates through the robot's graph, identifying and traversing RT edges, and inserting vertices and edges into an IGraph object.", + "description": "Within the SpecificWorker class recursively traverses the graph by following RT edges from the starting node until reaching the robot's ID or a maximum depth limit is reached. It inserts vertices and edges into an igraph handle for each iteration.", "params": [ { "name": "node_id", "type_name": "int", - "description": "A unique identifier for the node being traversed." + "description": "The unique identifier of the node to be traversed." } ], "returns": null, "name": "traverse_graph", "location": { - "start": 460, - "insert": 462, + "start": 490, + "insert": 492, "offset": " ", "indent": 8, "comment": null @@ -1566,23 +1581,23 @@ "docLength": null }, { - "id": "426f44c0-7eaf-b78f-2949-a354da7afe47", + "id": "9941450b-8dc1-aaac-2849-a5d941339841", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Iterates through the successors of a given vertex in a graph, and inserts vertices and edges into a data structure called `dsr` if the successor has a higher level than the current vertex and its name is not already present in `dsr`.", + "description": "Navigates through the graph by starting at a given vertex, identifying its successors, and inserting vertices and edges to preserve the DSR property of the graph. It then recursively traverses the successor vertices until no more updates are needed.", "params": [ { "name": "node", "type_name": "igraphVertex", - "description": "Represented by an index." + "description": "Representing an existing vertex in the graph." } ], "returns": null, "name": "traverse_igraph", "location": { - "start": 470, - "insert": 471, + "start": 500, + "insert": 501, "offset": " ", "indent": 8, "comment": null @@ -1592,23 +1607,23 @@ "docLength": null }, { - "id": "6537e0aa-a290-a4aa-e34a-a6fcf04c5f78", + "id": "225fa64e-4fe1-6387-0640-047b650806c6", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Adds a new vertex to an igraph graph, updating its attributes and linking it with other vertices based on their names.", + "description": "Adds a new vertex to an igraph graph, handling various attributes and edges based on their types and values.", "params": [ { "name": "node", "type_name": "igraphNode", - "description": "Passed the vertex to be added to the graph." + "description": "Passed to insert a new vertex into an existing graph." } ], "returns": null, "name": "insert_igraph_vertex", "location": { - "start": 483, - "insert": 484, + "start": 513, + "insert": 514, "offset": " ", "indent": 8, "comment": null @@ -1618,28 +1633,28 @@ "docLength": null }, { - "id": "bf521f4e-8469-df93-ed46-38f37fa5b135", + "id": "776dd3e6-e846-cd8c-9b46-91072201c62e", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Creates a new node in the graph and links it to its parent node by setting the `parent` attribute. It also sets other attributes based on the input node data and inserts the new node into the graph using the `insert_node` method.", + "description": "Inserts a new node into a graph, updating the node's attributes and parent relationship.", "params": [ { "name": "parent_name", "type_name": "str", - "description": "Representative of the name of a node to which a new vertex will be added as its parent." + "description": "Required, which refers to the name of a vertex in the graph that will serve as the parent for the new node being inserted." }, { "name": "node", "type_name": "Node", - "description": "Passed as an instance of the class representing a graph node, containing attributes such as agent ID and type." + "description": "Passed as an instance of the Node class representing a vertex to be inserted into the graph." } ], "returns": null, "name": "insert_dsr_vertex", "location": { - "start": 518, - "insert": 520, + "start": 548, + "insert": 550, "offset": " ", "indent": 8, "comment": null @@ -1649,23 +1664,23 @@ "docLength": null }, { - "id": "e2dcf73b-935c-9fb6-d447-fc3b778deaae", + "id": "cb8df5c5-bbfa-d99a-604f-44caea2e4e59", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Adds an edge to an igraph graph, specifying the origin and destination nodes, as well as the rotation and translation attributes of the edge.", + "description": "Adds an edge to an igraph object, specifying the origin and destination nodes, as well as the rotation and translation values.", "params": [ { "name": "edge", "type_name": "igraphEdge", - "description": "Used to represent an edge in the graph. It has attributes such as `origin`, `destination`, `attrs`, and `value`." + "description": "Passed as a whole object, containing attributes such as origin, destination, rt translation, and rotation euler xyz." } ], "returns": null, "name": "insert_igraph_edge", "location": { - "start": 532, - "insert": 535, + "start": 562, + "insert": 565, "offset": " ", "indent": 8, "comment": null @@ -1675,28 +1690,28 @@ "docLength": null }, { - "id": "a24f000e-1b16-fe81-7549-3dd2b8700dc0", + "id": "c9505cbd-5885-1895-b749-53af73e7aa35", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Modifies an existing edge in a graph based on information from two nodes, adding new attributes for the RT translation and rotation of the edge.", + "description": "Modifies an existing edge in a graph by adding new attributes representing RT translation and rotation, and then inserts or assigns the updated edge to the graph.", "params": [ { "name": "org", "type_name": "Agent", - "description": "Used to specify the origin node of the edge to be inserted." + "description": "Either None or an existing node object in the graph, representing the source node of the edge to be inserted." }, { "name": "dest", "type_name": "Agent", - "description": "Used to represent the destination node or edge in the graph." + "description": "Used to specify the target node in the graph for the new edge." } ], "returns": null, "name": "insert_dsr_edge", "location": { - "start": 544, - "insert": 547, + "start": 574, + "insert": 577, "offset": " ", "indent": 8, "comment": null @@ -1706,17 +1721,17 @@ "docLength": null }, { - "id": "2e01c904-f5c2-73b3-6a47-71c9916a753a", + "id": "bd3d9b86-fbfb-58a0-f047-f251b71e65aa", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Clears the axis, layouts the graph using Kamada-Kawai layout algorithm, and then plots the vertices and edges with grey lines and arrowheads pointing towards the nodes. It also displays node names using text annotation.", + "description": "Clears the axis, layouts the graph using the Kamada-Kawai algorithm, and then plots the vertices and edges with labels and arrows. It also displays node names.", "params": [], "returns": null, "name": "draw_graph", "location": { - "start": 574, - "insert": 575, + "start": 604, + "insert": 605, "offset": " ", "indent": 8, "comment": null @@ -1726,26 +1741,26 @@ "docLength": null }, { - "id": "5f26e45d-f00c-1b98-1f4b-5af6c11f2db2", + "id": "1c5969a2-d7a8-9fb8-154b-05151508cdac", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Determines the room ID of an element with the given node ID, retrieving the attribute value from the node's attributes and returning it if successful, or -1 otherwise.", + "description": "Determines the room number associated with a given node ID and returns it if found, or -1 otherwise.", "params": [ { "name": "node_id", - "type_name": "int", - "description": "Passed as an argument to the function representing a unique identifier for an element in the graph." + "type_name": "str", + "description": "Passed as an argument to the function, representing the ID of a node in the graph." } ], "returns": { "type_name": "int", - "description": "The room ID corresponding to the given node ID when the node has the \"room_id\" attribute, or -1 otherwise." + "description": "The room ID of a given node or -1 if an error occurs when trying to retrieve the room ID." }, "name": "check_element_room_number", "location": { - "start": 599, - "insert": 600, + "start": 629, + "insert": 630, "offset": " ", "indent": 8, "comment": null @@ -1755,26 +1770,26 @@ "docLength": null }, { - "id": "af524d88-6016-9cb4-f54b-329cbba2d34f", + "id": "8e705cf8-c7fa-4f9f-854a-5426294aa78f", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Within the SpecificWorker class checks the \"level\" attribute of a node and returns its value if found, or -1 otherwise. It also performs additional actions related to robot position and room connections in the internal graph.", + "description": "Within the SpecificWorker class determines an element's level based on an attribute value and returns that value if found, or -1 if not.", "params": [ { "name": "node_id", "type_name": "str", - "description": "Used to represent the unique identifier of a node in the graph." + "description": "Used to identify the node whose element level will be checked." } ], "returns": { "type_name": "int", - "description": "The level of an element with the given node ID." + "description": "Element level of a node." }, "name": "check_element_level", "location": { - "start": 608, - "insert": 609, + "start": 638, + "insert": 639, "offset": " ", "indent": 8, "comment": null @@ -1784,23 +1799,23 @@ "docLength": null }, { - "id": "5fe9df41-dec5-989d-6c49-d290d3b59d9c", + "id": "8f9ba2ca-0a1e-0386-d74e-d1179313cade", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Retrieves information about a room node in a graph, specifically its ID and the IDs of its adjacent nodes with a certain edge type, and then prints out these values and attempts to retrieve a translation attribute.", + "description": "Generates an image of a room based on its node ID, retrieves and prints the attributes of the room node, and draws the room polygon and doors.", "params": [ { "name": "room_node_id", "type_name": "str", - "description": "Used to get a specific node from the graph." + "description": "Used to retrieve a Node object representing the room node for which a picture should be generated." } ], "returns": null, "name": "generate_room_picture", "location": { - "start": 651, - "insert": 653, + "start": 681, + "insert": 683, "offset": " ", "indent": 8, "comment": null @@ -1810,23 +1825,23 @@ "docLength": null }, { - "id": "8f2759df-b075-8896-614a-8578e1b2c573", + "id": "097b5487-3516-2698-aa47-fa49c84dc627", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Creates a new edge in the graph with the current room ID and assigns it to the `self.g` variable, inserting or assigning the edge if it does not already exist.", + "description": "Inserts or assigns an edge into the graph, with the `room_id` as the tail id and the `self.agent_id` as the head id.", "params": [ { "name": "room_id", "type_name": "str", - "description": "Provided as an argument to the function by the caller, representing the room ID that the current edge is associated with." + "description": "A unique identifier for a particular room." } ], "returns": null, "name": "insert_current_edge", "location": { - "start": 686, - "insert": 688, + "start": 716, + "insert": 718, "offset": " ", "indent": 8, "comment": null @@ -1836,28 +1851,28 @@ "docLength": null }, { - "id": "68534604-01c8-c8b5-fe4b-df8ff1274feb", + "id": "07b3bda5-b24d-fa91-524d-c53e1ada98da", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Updates the state of an affordance node based on its ID and type. If the active affordance node's state is \"completed\" and its active status is false, it transitions to the \"crossed\" state.", + "description": "Updates an affordance node's state based on its active ID, printing various attributes and changing the node's state to \"crossed\" if it is completed and not active.", "params": [ { "name": "id", "type_name": "int", - "description": "Representing the unique identifier for the node to be updated." + "description": "Used to identify the node for which the affordance state should be updated." }, { "name": "type", "type_name": "str", - "description": "Used to indicate the type of the node being updated, which can be either \"active\" or \"completed\"." + "description": "Used to identify the node's type." } ], "returns": null, "name": "update_node", "location": { - "start": 700, - "insert": 701, + "start": 730, + "insert": 731, "offset": " ", "indent": 8, "comment": null @@ -1867,33 +1882,33 @@ "docLength": null }, { - "id": "6d818941-6870-1f92-724a-6a32635de3b4", + "id": "67cdf7cd-b880-0684-484f-a4a4089a786b", "ancestors": [ - "76fa4ef8-3f83-f299-c341-308421fee7c7" + "0a067d35-4dda-36b4-5c41-7f6586689420" ], - "description": "Updates the information about the robot's edge when it moves to a new node and the type of the edge is RT(room transition).", + "description": "Updates an edge in the graph based on the type and values of its arguments. If there is no current edge with the specified type, it inserts a new edge and sets it as the current one.", "params": [ { "name": "fr", "type_name": "int", - "description": "The ID of the room exit door." + "description": "Used to represent the ID of the current room door node that the function operates on." }, { "name": "to", "type_name": "int", - "description": "Passed to identify the destination node for updating the edge." + "description": "The ID of the target node to which the edge is to be updated." }, { "name": "type", "type_name": "str", - "description": "Representing the type of edge to be updated, which can be either \"RT\" or \"PT\"." + "description": "Used to specify the type of edge to be updated, which can be either \"RT\" or \"current\"." } ], "returns": null, "name": "update_edge", "location": { - "start": 715, - "insert": 719, + "start": 745, + "insert": 749, "offset": " ", "indent": 8, "comment": null diff --git a/.komment/komment.json b/.komment/komment.json index 0db52d3d..73ea794c 100644 --- a/.komment/komment.json +++ b/.komment/komment.json @@ -1,11 +1,12 @@ { "meta": { "version": "1", - "updated_at": "2024-07-04T16:02:07.320Z", + "updated_at": "2024-07-08T15:05:30.287Z", "created_at": "2024-07-03T15:48:27.349Z", "pipelines": [ "50c96b7c-ad3b-4b93-ae19-7f6fbd5637f7", - "9e4d0253-62f2-4920-b2d3-607cb4cf3291" + "9e4d0253-62f2-4920-b2d3-607cb4cf3291", + "510e9758-a82c-441e-9653-dc071bb409eb" ] }, "lookup": [ diff --git a/agents/g2o_agent/src/specificworker.py b/agents/g2o_agent/src/specificworker.py index d53efd4b..2dceb299 100644 --- a/agents/g2o_agent/src/specificworker.py +++ b/agents/g2o_agent/src/specificworker.py @@ -49,123 +49,122 @@ class SpecificWorker(GenericWorker): """ - Performs real-time visual odometry estimation for a robot using a G2O graph - and RT communication. It initializes a graph, adds nominal corners, and updates - edges with measured data from an RT communication agent. + Is responsible for processing robot poses and creating a graph representation + of the environment for a specific worker in a multi-robot system, including + handling edges, vertices, odometry, and room changes. Attributes: - Period (float): Used to control the interval at which the worker performs - its tasks, with a default value of 1 second. It can be used to adjust - the worker's execution speed. - agent_id (int): A unique identifier assigned to each agent in the system. - g (Graph): Used for representing the graph of a robot's motion, where each - node represents a pose of the robot and each edge represents a motion - of the robot between two poses. The graph is used to compute the - marginal probability distribution of the robot's motion given its - initial position and velocity. - startup_check (QTimersingleShot): Used to start a quit timer after 200 - milliseconds of inactivity, which allows the worker to exit gracefully - when no further work is available. - rt_api (str): 8 characters long. It stores a string value that represents - the ROS launch file path for the RT node. - inner_api (instance): Used as a reference to the inner API object, which - is responsible for handling the actual work of the worker. - odometry_node_id (int): 0-based index of the node representing the robot's - odometry information in the graph. It is used to identify the node in - the graph that represents the robot's current position and velocity. - odometry_queue (3D): A queue that stores the odometry data of the robot, - which is used to update the graph and calculate the robot's pose. - last_odometry (3D): Used to store the last known odometry value of the - robot, which is used for calculating the displacement and covariance - matrix. - g2o (3D): A G2O graph, which is a data structure used to represent the - robot's pose in a 3D environment. It stores the information of the - robot's pose in a set of nodes and edges, where each node represents - a location in space and each edge represents a connection between two - locations. The `g2o` attribute is used to perform optimization tasks - such as finding the robot's pose that minimizes a cost function. - odometry_noise_std_dev (float): Used to represent the standard deviation - of odometry noise in the worker's graph. It is used in the `get_displacement` - method to compute the displacement between two poses based on the - odometry noise. - odometry_noise_angle_std_dev (floatingpoint): 1.5 by default, which - represents the standard deviation of angle noise in the odometry - measurement of a robot. It affects the accuracy of the robot's positioning - and movement tracking. - measurement_noise_std_dev (float): 10% of the robot's minimum distance - from its reference configuration to its target position, indicating - the standard deviation of measurement noise in the robot's pose estimation. - last_room_id (int): Used to store the previous room ID that was visited - by the agent before the current room. It is updated when the agent - moves to a new room, allowing the worker to detect changes in the environment. - actual_room_id (int): Used to store the current room ID of the agent, which - is updated when the robot moves from one room to another. - elapsed (int): Used to store the time elapsed since the last call to the - `startup_check` function, which is called every 0.1 seconds. It is - used to determine when to quit the application. - room_initialized (bool): Used to keep track of whether the current room - has been initialized or not. It's set to False when a new room is - entered, and True otherwise. - iterations (int): Used to keep track of the number of iterations performed - by the worker. It increases by one each time a new iteration is performed - and can be accessed or modified from within the worker's methods. - hide (str): Used to specify whether a worker should be hidden or not when - multiple workers are running simultaneously. - init_graph (bool): Used to indicate whether the graph has been initialized - or not. It is set to True when the graph is initialized and False - otherwise, allowing the worker to distinguish between initialized and - uninitialized graphs. + Period (instance): Used to specify the time interval between updates of + the worker's state, allowing for more efficient processing of large datasets. + agent_id (int): Used as a identifier for the agent that owns the specific + worker. + g (Graph): Used to store the graph representation of the environment, which + is updated based on the worker's observations and actions. + startup_check (QTimersingleShot): Used to check if the application should + quit after a certain time has elapsed since startup. It's used to + implement the main loop of the worker, where the worker periodically + checks if it should quit based on the current time. + rt_api (instance): Used to store the RT API object, which is used for + real-time data processing and transmission. + inner_api (instance): Used to store the inner API object of the worker, + which allows for communication between the worker and the main thread. + odometry_node_id (int): 3 in this example, indicating that the node with + ID 3 represents the odometry information. + odometry_queue (3D): A list of tuples containing the current pose of the + robot, which is updated at each step by the worker. The queue stores + the last `N` poses for each node in the graph, where `N` is a user-defined + parameter. + last_odometry (3D): Used to store the last known odometry of the robot, + which can be used for visualization and debugging purposes. + g2o (3D): Used to represent the pose (position, orientation, and scale) + of a robot or other object in 3D space. It is used in conjunction with + the `get_covariance_matrix` method to compute the covariance matrix + of the robot's position and orientation. + visualizer (instance): A reference to an instance of the `Visualizer` + class, which provides a user interface for visualizing the robot's + pose and other relevant data in real-time. + odometry_noise_std_dev (float): 0.1 by default, which represents the + standard deviation of the noise added to the odometry values during + the simulation for more realistic results. + odometry_noise_angle_std_dev (float): 0.8 by default, which represents the + standard deviation of the angular noise in the odometry measurement. + It affects the optimization process by controlling the spread of the + angle values in the graph. + measurement_noise_std_dev (float): Used to represent the standard deviation + of noise in the robot's measurements. It is used in the worker's + implementation to scale the measurement values to account for the noise + in the data. + last_room_id (int): Used to store the last room ID seen before the agent + switched rooms, which is used to handle room changes during navigation + tasks. + actual_room_id (int): Used to keep track of the current room id that the + worker is in, it's updated when the worker moves to a new room. + elapsed (int): Used to track the elapsed time since the worker was created, + which can be used to control the worker's execution time and prevent + it from running for too long. + room_initialized (bool): Used to track whether the robot's room has been + initialized or not. It is set to False when the robot moves to a new + room, and True otherwise. + iterations (int): Used to count the number of iterations performed by the + worker during its execution. It is incremented each time the worker + processes a new iteration of the graph optimization problem. + hide (str): Used to indicate whether the worker should be hidden or not, + defaulting to False. + init_graph (bool): Used to keep track of whether the graph has been + initialized or not, during the processing of the ROS bag file. It's + set to True when the graph is first constructed and False when the + graph is reconstructed after a change in the environment. current_edge_set (bool): Used to indicate whether the current edge set has - been computed or not. It is set to true when the edge set is first - computed and false otherwise, so that only new computations are performed. - first_rt_set (bool): Set to `True` when a new RT translation and rotation - are detected, and `False` otherwise. It indicates whether the worker - has received its first RT set or not. - translation_to_set (3D): Used to store the translation from the robot's - reference frame to the set frame. - rotation_to_set (3D): Used to store the rotation of a robot's end effector - relative to its base frame, which is necessary for setting the desired - orientation of the robot's end effector. - room_polygon (numpyarray): Used to store the coordinates of a room's - boundary in a list of (x,y) tuples. - initialize_g2o_graph (instance): Used to initialize a Graph2ONode graph - for a specific robotic arm. It creates nodes, edges, and defines the - room node, and sets up the optimization algorithm and visualization tools. - rt_set_last_time (float): Used to store the last time when a RT set was - set by the worker, which is used to filter out unnecessary RT sets. - rt_time_min (float): Set to the minimum time between two consecutive RT - sets. It is used to determine when to reset the translation and rotation - values for RT tracking purposes. - timer (QTimer): Used to schedule periodic updates of the graph with the - current robot state. It is set to single shot every 200 milliseconds, - allowing for real-time updates of the graph. - compute (instance): Used to compute the marginal probability distributions - of the robot's state given its current observations. It takes an - argument `vertex` which is a vertex in the graph representing the - robot's state, and returns a tuple containing the marginal probabilities - and the Hessian matrix of the robot's state. - update_node_att (update): Called when a new node with the specified id is - added to the graph, or an existing node's attributes are updated. It - updates the odometry queue based on the new node's attributes and sets - the room initialization flag accordingly. - update_edge (edge): Used to update the attributes of an edge in a graph - when the edge's node types change or when a new edge is added to the - graph. It sets the `rt_translation` and `rotation_euler_xyz` attributes - of the edge based on the new node types. - update_edge_att (attribute): Used to update the attributes of an edge in - a graph. It takes the edge ID, the new value for the attribute, and - the name of the attribute as input parameters. + been updated or not. It's used to track the updates of the edges in real-time. + first_rt_set (bool): Set to `True` when the worker receives its first RT + (Real-time) edge from a "room" node, indicating that the worker should + start tracking the RT translation and rotation for this room. + translation_to_set (3D): Used to store the translation of a robot to a set + point, calculated based on the RT messages it receives. It is updated + in the `update_edge_att` method. + rotation_to_set (3D): Used to store the rotation of the robot's end effector + with respect to a set reference frame. It is updated whenever the + robot's pose changes, and is used in the computation of the robot's + covariance matrix. + room_polygon (Polygon): Used to store the polygon representation of a room + in the environment. It is used for collision detection and other + purposes related to navigation and mapping. + security_polygon (3D): Used to store the security polygon of the robot, + which is a convex polytope used for collision detection and avoidance. + initialize_g2o_graph (instance): Used to initialize the graph2onedges (G2O) + graph during the initialization of the worker, allowing for faster + computation of marginals in the optimize method. + rt_set_last_time (int): Used to store the time when the first RT set was + observed by the worker, for updating the translation and rotation of + the robot to the set. + rt_time_min (float): Defined as `self.rt_time_min = 10`. It represents the + minimum time interval between two RT sets for the robot to be considered + in the RT algorithm. + timer (QTimer): Used to create a timer that triggers the worker's method + to update the graph every 200 milliseconds. + compute (lambda): Called when a task is assigned to the worker. It takes + the task as input, processes it using a provided function, and returns + the result as output. + update_node_att (Python): Used to update the attributes of a node in the + graph when its ID matches the specified ID. + update_edge (update): Called whenever an edge's attributes change. It + checks if the edge is a "room" edge and updates the + "translation to set", "rotation to set", or "first RT set" variables + based on the edge's type and + attributes. + update_edge_att (edge): Used to update the attributes of an edge in a graph. """ def __init__(self, proxy_map, startup_check=False): """ - Initializes a SpecificWorker instance, defining its attributes and connections - to graphs, timers, and signal handlers. + Initializes member variables and sets up event handling for signals related + to the graph, nodes, edges, and attributes. Args: - proxy_map (dict): Used to store a mapping from agent IDs to graphs. - startup_check (Optionalbool): Used to check if the robot's graph has - already been initialized before starting the main loop. + proxy_map (dict): Used to map agent id's to their corresponding DSRGraph + objects. + startup_check (bool): Used to determine whether to run the `startup_check` + method or not when initializing the SpecificWorker object. """ super(SpecificWorker, self).__init__(proxy_map) @@ -186,7 +185,7 @@ def __init__(self, proxy_map, startup_check=False): self.last_odometry = None # Initialize g2o graph with visualizer self.g2o = G2OGraph(verbose=False) - # self.visualizer = G2OVisualizer("G2O Graph") + self.visualizer = G2OVisualizer("G2O Graph") self.odometry_noise_std_dev = 1 # Standard deviation for odometry noise self.odometry_noise_angle_std_dev = 1 # Standard deviation for odometry noise @@ -210,6 +209,7 @@ 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 @@ -239,10 +239,8 @@ def setParams(self, params): @QtCore.Slot() def compute(self): """ - Generates high-quality summaries of Java code that is given to it. It takes - into account the robot's odometry, computes the translation and rotation - of the robot relative to a reference frame, and adds the information to - the graph. It also updates the landmark in the graph. + Updates the robot's position, orientations, and security polygon based on + the odometry data and the RT graph. """ if time.time() - self.elapsed > 1: @@ -253,47 +251,70 @@ 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") + + 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) + + 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: + 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]) + chi_value = self.g2o.optimize(iterations=50, verbose=False) last_vertex = self.g2o.optimizer.vertices()[self.g2o.vertex_count - 1] @@ -308,7 +329,7 @@ def compute(self): # 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) + 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] @@ -324,7 +345,7 @@ def compute(self): # print("Time elapsed compute:", timfe.time() - init_time) 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") + # 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() @@ -338,31 +359,33 @@ def add_noise(self, value, std_dev): return value + np.random.normal(0, std_dev) def initialize_g2o_graph(self): + # print("Initializing g2o graph") + # get robot pose in room """ - Initializes a Graph2O graph for a specific robot and room, adding nominal - corners and measuring them to create a polygonal representation of the - room. It also sets the robot's initial position and orientation in the graph. + Initializes a Graph-based Object Recognition (G2O) graph for a specific + room by: + 1/ Finding the corners of the room using ROS topics. + 2/ Adding nominal corners to the G2O graph. + 3/ Adding fixed poses to the G2O graph for the robot and doors in the room. Returns: - bool: 1 if the g2o graph was successfully initialized with the given - room id, and 0 otherwise. + bool: 1 if the initialization of the g2o graph was successful, and 0 + otherwise. """ - print("Initializing g2o graph") + 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: @@ -382,55 +405,184 @@ 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 = 300 + 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 + 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) + 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 the displacement of a robot based on its odometry data, updating - the robot's position and orientation using the lateral, angular, and linear - displacements. + Calculates the displacement of an object based on its odometry data, + computing the lateral displacement, forward displacement, and angular + displacement using a moving window approach. Args: - odometry (stdvectordouble): 3D vector representing the robot's position, - orientation, and velocity at a given time stamp. + odometry (3element): Used to store the robot's motion information at + each time step, including its linear displacement, lateral + displacement, and angular displacement. Returns: - triplet: 3 elements long (desplazamiento lateral, displacement avance - and angular displacement). + 3element: Displacement in three directions (lateral, advance and + angular) calculated using the odometry data from a queue. """ desplazamiento_avance = 0 @@ -453,18 +605,18 @@ def get_displacement(self, odometry): def get_covariance_matrix(self, vertex): """ - Computes the covariance matrix of a set of vertices in a graph using the - G2O optimization algorithm. It takes a vertex as input, returns the computed - covariance matrix and a result flag indicating whether the computation was - successful. + Computes the covariance matrix for a given vertex in a graph, using an + optimization algorithm to compute the marginals of the vertices and then + constructing the covariance matrix. Args: - vertex (g2oVertex): Passed to compute marginals for computing covariance - matrix. + vertex (g2overtex): Used to compute the covariance matrix of the + vertices in the graph. Returns: - tuple: 2-element, containing the result of computing covariance matrix - and the marginals of the vertices. + tuple: 2-element, where the first element is a boolean value indicating + whether the covariance matrix was computed or not and the second element + is the actual covariance matrix. """ cov_vertices = [(vertex.hessian_index(), vertex.hessian_index())] @@ -481,12 +633,12 @@ def get_covariance_matrix(self, vertex): def visualize_g2o_realtime(self, optimizer): """ - Visualizes a G2O optimization problem in real-time, loading the problem - from an file and plotting the vertices and edges as 3D scatter plots while - updating the display with small intervals of time. + Within the `SpecificWorker` class loads an G2O file, visualizes its vertices + and edges in 3D, and updates the visualization in real-time as the optimizer + processes new measurements. Args: - optimizer (instance): Used to load G2O files for visualization in real-time. + optimizer (instance): Used to load G2O files. """ plt.ion() @@ -535,13 +687,13 @@ def update_node_att(self, id: int, attribute_names: [str]): # print("INIT GRAPH") """ - Updates the attributes of a node in a graph, specifically the `odometry_queue` - attribute, when the node's ID matches the `odometry_node_id`. + Updates the attributes of a node in a graph, specifically the odometry + node, by appending values to an odometry queue. Args: - id (int): Used to identify the node for which attribute values are - being updated. - attribute_names ([str]): A list of attribute names to update in the node. + id (int): Used to represent the node ID of interest for updating attributes. + attribute_names ([str]): An array of strings representing the names + of attributes to be updated on the node. """ if id == self.odometry_node_id: @@ -562,18 +714,21 @@ def update_node(self, id: int, type: str): # self.room_initialized = True if self.initialize_g2o_graph() else False # self.init_graph = True """ - Updates the node with the specified ID, based on the type of node it represents. + Updates an unknown node with the specified ID, performing different actions + based on the type of node it represents. Args: id (int): Used to identify the node being updated. - type (str): Used to specify the node type. + type (str): Used to specify the node's type, with possible values being + "corner". """ pass def delete_node(self, id: int): """ - Deletes a node from the graph represented by the `SpecificWorker` class. + Deletes a node from the graph represented by the `SpecificWorker` class, + setting the `room_initialized` attribute to `False`. Args: id (int): Used to identify the node to be deleted. @@ -589,14 +744,18 @@ def delete_node(self, id: int): def update_edge(self, fr: int, to: int, type: str): """ - Updates the room ID and sets the current edge set based on the provided - edge type and node types. + Updates the room ID and sets the current edge set to true when the type + is "current" or "RT" and the source node is a room and the target node is + "Shadow". It also sets the translation and rotation to set variables when + RT is set for the first time. Args: - fr (int): Representative of the start node of an edge in a graph. - to (int): Used to represent the target node of an edge in the graph. - type (str): Used to specify the type of edge to update, either "current" - or "RT". + fr (int): Used to represent the ID of the current node being processed + in the graph. + to (int): The id of the next node in the graph that the edge is pointing + to. + type (str): Used to determine which room the edge updates are for, + either the current room or RT (room to Shadow) """ if type == "current" and self.g.get_node(fr).type == "room": @@ -628,15 +787,13 @@ def update_edge_att(self, fr: int, to: int, type: str, attribute_names: [str]): def delete_edge(self, fr: int, to: int, type: str): """ - Deletes an edge from a graph, specified by the vertex indices `fr` and - `to`, and the edge type. + Deletes an edge from a graph with specified source and target nodes, based + on the type of edge. Args: - fr (int): Used to represent the starting vertex index of an edge to - be deleted. - to (int): Used to indicate the destination node ID for edge deletion. - type (str): Used to specify the edge type to be deleted, with possible - values 'in' or 'out'. + fr (int): Given a value of 13. + to (int): Specified as an index of an edge to be deleted. + type (str): Used to specify the edge type to be deleted. """ pass 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 ac88cd3c..5b497d32 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 @@ -14,35 +14,35 @@ class LongTermGraph: """ - Performs a series of actions: it reads and computes a graph from a text file, - draws the graph using Matplotlib, and provides methods for navigating and - querying the graph. + Computes and draws a graph representation of a long-term memory space, allowing + for door-based navigation and metric reconstruction. It takes in a set of rooms + and their connections as well as a base room name and computes the metric map + between them. Attributes: - g (undetected): Used to store the graph data structure for computing the - metric reconstruction. - read_graph (method): Responsible for reading a graph from a file in Graphviz - format. It takes no arguments and returns a Graph object. - fig (matplotlibfigureFigure): Used to represent the figure object for - visualizing the graph. - ax (PyQt5QtWidgetsQGraphicsScene): Used to represent the graphical display - area where the graph will be drawn. - fig_2 (matplotlibfigureFigure): Used to store the figure object for the - graph display. - ax_2 (matplotlibFigure): Used to store a second axis object for plotting + g (Graph): Used to store a graph representation of a long-term spatial + memory task. It contains nodes, edges, and types (rooms, doors, walls) + that are used to compute the metric map. + read_graph (instance): Used to read a graph from a pickled file. It returns + a `Subgraph` object representing the graph. + fig (matplotlibfigureFigure): Used to store the figure object that will + be drawn with the graph. + ax (Axes): Used to store the axis object for plotting the graph. + fig_2 (MatplotlibFigure): Used to store a figure object used for drawing the graph. + ax_2 (Axis): Used to represent the graph's second axis, which is used for + the metric reconstruction + plot. It provides functions to set the title, labels, and limits for + this axis. """ def __init__(self, file_name): """ - Reads a graph from a file, creates two subplots in an matplotlib figure - for visualizing the graph and its metric reconstruction, and sets up labels - for the x- and y-axes. If the file is not found, it sets `self.g` to `None`. + In the LongTermGraph class reads a graph from a file, creates subplots for + metric reconstruction and LTSM display, and initializes the graph object. Args: - file_name (str): Used to specify the path of a graph file that contains - a directed multigraph, which will be read by the function and - processed for metric reconstruction. + file_name (str): Used to specify the name of the graph file to read. """ try: @@ -172,11 +172,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""" @@ -244,13 +250,14 @@ def check_point_in_map(self, rooms_map: dict, point: QPoint): def draw_graph(self, only_rooms=True): """ - Generates a graphical representation of a subgraph of a larger graph, based - on node and edge attributes. It creates a scatter plot of room and door - nodes, and plots edges as grey lines with node names annotated. + Generates a graph visualization of a subgraph of a larger graph, based on + node and edge types. It uses the `layout` parameter to specify the layout + algorithm and colors nodes and edges based on their type. Args: - only_rooms (bool): Used to filter out nodes that are not rooms, doors, - or walls. + only_rooms (bool): Used to filter the nodes and edges displayed on the + graph based on their types, with "room" being the default value + for only rooms are shown. """ self.ax_2.clear() @@ -312,14 +319,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 b15ebdac..813c7f3b 100644 --- a/agents/long_term_spatial_memory_agent/src/specificworker.py +++ b/agents/long_term_spatial_memory_agent/src/specificworker.py @@ -48,86 +48,85 @@ class SpecificWorker(GenericWorker): """ - Is responsible for processing specific types of nodes and edges in a graph, - such as room nodes and their corresponding doors, as well as affordance nodes - and their transitions. It also handles updating the graph's attributes and - inserting new edges. + Performs various tasks related to a specific worker in a multi-agent system, + such as checking for room occupation, updating node attributes, and generating + room images. It also handles insertion and deletion of edges and nodes, and + updates the state of the agent based on certain conditions. Attributes: - Period (float32): 0 by default. It represents the period of time taken for - a specific worker to complete its task, which can be used to determine - the next task to assign to the worker based on its current state. - agent_id (int): 0 by default, representing the agent's internal ID for - distinguishing it from other agents. - g (Graph): Used to store the graph data structure for the environment map - navigation. It contains the nodes, edges, and other attributes needed - for the worker's functionality. - update_node (attribute): Called when a node's attributes change. It updates - the state of the worker based on the new attributes. - update_edge (Edge): Used to update the edge attributes based on the robot's - movement. It takes three parameters: `fr`, `to`, and `type`, which - represent the from, to, and edge type respectively. The method updates - the edge attributes accordingly and sets the current edge as "current" - if there are no existing "current" edges. - startup_check (QTimersingleShot): Used to schedule the worker's main loop - to run after 200 milliseconds of starting the application. - rt_api (Attribute): Used to store the RT API of the worker, which is needed - for communication between the worker and the main agent. - inner_api (internal): Used by the worker to interact with its inner agent - API, which handles the actual robot control and sensor data processing. - robot_name (str): Used to store the name of the robot that the worker is - controlling. - robot_id (int): Used to store the ID of the robot that the worker represents. - last_robot_pose (Attribute): Used to store the last known pose of the robot - in the environment, which can be used for navigation and task completion - purposes. - state (str): Used to keep track of the worker's current state, either - "idle", "running", or "crossed". - affordance_node_active_id (int): Used to store the ID of the affordance - node that is currently active, indicating which affordance the worker - should perform. - exit_door_id (int): 0-based, representing the index of the door through - which the robot exits a room when it moves from that room to another. - room_exit_door_id (int): 14 by default. It represents the ID of the door - through which the robot exits a room. + Period (int): 200 milliseconds, which is the time interval between updates + of the worker's state. + agent_id (integerint): Used as a unique identifier for each agent in the + swarm. + g (Graph): Used to store the graph representation of the environment that + the worker is working on. It contains nodes, edges, and other attributes + that are relevant for the worker's task. + update_node (UpdateNode): Used to update the state of a node in the graph + based on its ID, type, and affordance state. + update_edge (str): Used to set a new current edge in the graph when there + is no existing current edge and the room node exists. + startup_check (QTimersingleShot): Used to start the application's shutdown + process after a certain period of time (200 milliseconds) since the + worker was created. + rt_api (str): Used for handling robot actions, specifically RT (Remote + Transport) actions. It provides a way to interact with the robot's + internal API to perform actions such as moving or manipulating objects. + inner_api (API): Used to access the internal API of the agent, allowing + the worker to interact with the agent's internal state and perform + actions such as generating room pictures or updating node attributes. + robot_name (str): Used to store the name of the robot agent in the environment. + robot_id (int): Used to store the ID of the robot node in the graph. It + is used to identify the current room and for RT edges. + last_robot_pose (Pose): Used to store the last known pose of the robot in + the environment, which is used for motion planning and collision detection. + robot_exit_pose (Pose): Used to store the pose of the robot when it exits + a room. + state (str): Used to store the current state of the worker, which can be + either "idle", "running", or "crossed". + affordance_node_active_id (int): Used to store the ID of the node representing + the affordance that the robot is currently interacting with. + exit_door_id (int): 0 by default, indicating the door through which the + robot exits a room when it moves to another room. + room_exit_door_id (int): 31 by default, representing the door ID of the + room that the worker is exiting. enter_room_node_id (int): Used to store the ID of the room node that the - robot enters when it moves from the previous room to the current room. - graph (Graph): Used to store the graph representation of the environment, - including nodes, edges, and their attributes. - vertex_size (int): Used to represent the size of a vertex in a graph, - indicating its importance or relevance in the graph structure. - not_required_attrs (list): Used to store the names of attributes that are - not required for the worker's functionality, i.e., they are optional - or not used in the implementation. - long_term_graph (Graph): Used to store the long-term graph of the agent, - which is updated at each iteration of the worker. It keeps track of - the agent's internal graph over time, allowing the worker to perform - actions based on the historical information of the environment. - global_map (ndarray): 2D, representing a map of the environment. It stores - the occupancy probabilities of each cell in the grid, which are used - to calculate the worker's attention and motion. + worker has entered. It is used to check if the worker has reached the + goal state by comparing it with the ID of the goal room node. + graph (Graph): Used to represent the graph of nodes and edges in the + environment, which is updated based on the worker's actions and observations. + vertex_size (int): 16 by default, indicating the size of each vertex in + the graph. + not_required_attrs (list): Used to store a list of node attributes that + are not required for the worker's functionality. It helps in avoiding + unnecessary computation and improving performance by skipping the + retrieval of these attributes during node traversal. + long_term_graph (Graph): Used to store the long-term graph of the environment, + which represents the entire state of the environment over time. + global_map (npndarray): Used to store a map of the environment, which is + used for navigation and other purposes. insert_current_edge (Edge): Used to insert a new edge into the graph with - a specified type (current) and source and destination nodes, signaling - that the current room has been reached and the agent should move on - to the next task. - timer (QTimer): Used to schedule the worker's startup check after a delay - of 200 milliseconds. - compute (instance): Used to compute the next action for the agent based - on the current state of the environment. It takes into account the - agent's attributes, such as its position and orientation, and returns - a dict with the next action as key and the corresponding value as value. + the current room as the source node and the robot node as the destination + node, indicating that the robot has entered the current room. + timer (QTimer): Used to schedule a call to the `startup_check()` function + after 200 milliseconds of starting the application, which signals the + agent to start its work. + compute (instance): Used to compute the cost of the robot moving from its + current state to a new state. It takes into account factors such as + distance, obstacles, and door states. """ def __init__(self, proxy_map, startup_check=False): """ - Sets up the internal structure and event connections for a SpecificWorker - instance, including its graph, node ID, and timer for computing updates. + Initializes an instance of the SpecificWorker class by setting up various + graph structures, defining variables, and connecting signals for updating + node and edge positions. Args: - proxy_map (igraphGraph): Used to specify the graph for this specific - worker instance. - startup_check (bool): Used to check if any of the required nodes are - present in the graph before starting the agent's inner loop. + proxy_map (igraphGraph): Used to specify the graph that represents the + environment for the worker agent. + startup_check (bool): Used to check if the agent has already started. + If set to `False`, it will call the `startup_check` method instead, + which initializes the internal state of the agent. """ super(SpecificWorker, self).__init__(proxy_map) @@ -158,6 +157,7 @@ def __init__(self, proxy_map, startup_check=False): 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" @@ -196,12 +196,14 @@ def __del__(self): def setParams(self, params): """ - Sets parameters and updates node attributes in a graph. It removes a - self-edge from a room, stores an exit door ID, and assigns other_side_door - attributes to both doors leading to the new room. + Sets parameters and modifies the state of a Room object by removing a + self-edge, storing an exit door ID, setting attribute values for doors, + and reading an entrance door node to add an other_side_door attribute with + the name of the exit door in the new room. Args: - params (object): Passed to the function for configuration purposes. + params (object): Passed in from outside the function to be processed + or modified. Returns: Boolean: True. @@ -240,9 +242,8 @@ def compute(self): # print(origin_level, destination_level) """ - Performs various actions based on the current state of the worker, including - idle, crossing, crossed, initializing room, known room, initializing doors, - storing graph, and removing. + Determines the state of a worker based on the current state of the graph + and performs appropriate actions according to the state. """ match self.state: @@ -267,8 +268,8 @@ def compute(self): 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 """ - Determines if the agent is entering or exiting a room based on the affordance - graph and updates the agent's state and door associations accordingly. + Generates high-quality summaries of Java code given to it, checks if any + "current" edge exists, and if not, associates doors based on their proximity. """ aff_cross_nodes = [node for node in self.g.get_nodes_by_type("affordance") if node.attrs["active"].value == True] @@ -307,15 +308,24 @@ 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]] + self.robot_exit_pose = robot_pose # 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 @@ -323,8 +333,6 @@ 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, @@ -333,24 +341,22 @@ def idle(self): # 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) @@ -363,8 +369,10 @@ def idle(self): def crossed(self): # Get parent node of affordance node """ - Determines if the agent has reached a door and, if so, updates its state - based on the door's properties. + Manages the node and edge properties for a worker class called `SpecificWorker`, + which is an extension of `GenericWorker`. It retrieves the affordance node, + checks if it has a parent, and updates the exit door ID node with the + connected room name. """ affordance_node = self.g.get_node(self.affordance_node_active_id) @@ -378,7 +386,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: @@ -389,8 +397,9 @@ def initializing_room(self): # Get room nodes """ - Determines and stores the ID of the room node to enter when it is called, - and updates the state of the worker to "initializing doors". + Determines if there are any rooms other than the current room that the + worker is in, and if so, sets the enter room node ID and inserts an edge + to it. It then sets the state to "initializing doors" and prints a 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] @@ -407,77 +416,102 @@ def initializing_room(self): def known_room(self): # Get other side door name attribute """ - Performs a series of actions to handle a robot's movement through a door, - including: - 1/ Finding the other side room node in the graph. - 2/ Creating a new edge and vertex for the robot's new location. - 3/ Deleting the original edge connecting the robot to the room. + In the `SpecificWorker` class is responsible for handling the scenario + where the robot needs to move to another room through a door, and there + is no direct path from the current room to the desired room. It traverses + the graph to find the shortest path to the desired room, updates the node's + parent attribute, and inserts a new edge in the graph to connect the robot + to the desired room. """ 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 + 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: - 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) - - - except Exception as e: - print("No other side door room node found") - print(e) - return + # 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 node found") + print("No other side door room node found") print(e) return self.state = "removing" @@ -485,9 +519,9 @@ def known_room(self): def initializing_doors(self): # Check if node called "room_entry" of type room exists """ - 1) retrieves exit edges and matching same-side doors, 2) sets other side - door name and connected room name attributes for each exit door node, and - 3) associates doors between nodes using their names. + Identifies the doors connected to the current room and updates their + attributes with the names of the rooms they lead to, also associating them + through a pair of tuples. """ exit_edges = [edge for edge in self.g.get_edges_by_type("exit") if edge.destination == self.exit_door_id] @@ -530,15 +564,15 @@ def initializing_doors(self): def associate_doors(self, door_1, door_2): # Find each door in igraph and update attributes """ - Takes two door nodes as input and connects them by adding an edge between - them, while also updating the "other side door name" and "connected room - name" attributes of each node to reflect the connection. + Connects two doors in a graph by adding an edge between their nodes and + updating the "other side door name" and "connected room name" attributes + of each door node. Args: - door_1 (str): Represented as an tuple containing the name of the door - and its room, such as ("Door 1", "Room 1"). - door_2 (str): Represented as a string containing the name of the second - door to be associated with the graph. + door_1 (igraphNode): A reference to one of the doors in the graph, + which is used to establish a connection between two rooms. + door_2 (str): 2-dimensional list containing the name of the door and + its room information. """ try: @@ -560,8 +594,8 @@ def associate_doors(self, door_1, door_2): def store_graph(self): """ - Stores the graph represented by an instance of `GenericWorker` in a file - named "graph.pkl". + Stores the graph data in a file called "graph.pkl" when a room node is + found in the igraph data structure. """ actual_room_node = self.g.get_node(self.room_exit_door_id) @@ -580,10 +614,8 @@ def store_graph(self): def removing(self): # # Get last number in the name of the room """ - Removes edges from the graph that connect two nodes with the same room - number, and also deletes nodes that have only one connection to another - node with a room number of 200 (shadow nodes). The function then updates - the long-term graph and sets the state to "idle". + Removes edges and nodes from the graph based on certain room numbers, + updates the long-term graph, and changes the state to "idle". """ room_number = self.g.get_node(self.room_exit_door_id).attrs["room_id"].value @@ -613,11 +645,13 @@ def removing(self): def traverse_graph(self, node_id): # Mark the current node as visited and print it """ - Iterates through the robot's graph, identifying and traversing RT edges, - and inserting vertices and edges into an IGraph object. + Within the SpecificWorker class recursively traverses the graph by following + RT edges from the starting node until reaching the robot's ID or a maximum + depth limit is reached. It inserts vertices and edges into an igraph handle + for each iteration. Args: - node_id (int): A unique identifier for the node being traversed. + node_id (int): The unique identifier of the node to be traversed. """ node = self.g.get_node(node_id) @@ -630,13 +664,13 @@ def traverse_graph(self, node_id): def traverse_igraph(self, node): """ - Iterates through the successors of a given vertex in a graph, and inserts - vertices and edges into a data structure called `dsr` if the successor has - a higher level than the current vertex and its name is not already present - in `dsr`. + Navigates through the graph by starting at a given vertex, identifying its + successors, and inserting vertices and edges to preserve the DSR property + of the graph. It then recursively traverses the successor vertices until + no more updates are needed. Args: - node (igraphVertex): Represented by an index. + node (igraphVertex): Representing an existing vertex in the graph. """ vertex_successors = self.graph.successors(node.index) @@ -653,11 +687,11 @@ def traverse_igraph(self, node): def insert_igraph_vertex(self, node): """ - Adds a new vertex to an igraph graph, updating its attributes and linking - it with other vertices based on their names. + Adds a new vertex to an igraph graph, handling various attributes and edges + based on their types and values. Args: - node (igraphNode): Passed the vertex to be added to the graph. + node (igraphNode): Passed to insert a new vertex into an existing graph. """ self.graph.add_vertex(name=node.name, id=node.id, type=node.type) @@ -697,16 +731,14 @@ def insert_igraph_vertex(self, node): def insert_dsr_vertex(self, parent_name, node): # print("Inserting vertex", node["name"], node["type"]) """ - Creates a new node in the graph and links it to its parent node by setting - the `parent` attribute. It also sets other attributes based on the input - node data and inserts the new node into the graph using the `insert_node` - method. + Inserts a new node into a graph, updating the node's attributes and parent + relationship. Args: - parent_name (str): Representative of the name of a node to which a new - vertex will be added as its parent. - node (Node): Passed as an instance of the class representing a graph - node, containing attributes such as agent ID and type. + parent_name (str): Required, which refers to the name of a vertex in + the graph that will serve as the parent for the new node being inserted. + node (Node): Passed as an instance of the Node class representing a + vertex to be inserted into the graph. """ new_node = Node(agent_id=self.agent_id, type=node["type"], name=node["name"]) @@ -725,12 +757,12 @@ def insert_igraph_edge(self, edge): # Search for the origin and destination nodes in the graph """ - Adds an edge to an igraph graph, specifying the origin and destination - nodes, as well as the rotation and translation attributes of the edge. + Adds an edge to an igraph object, specifying the origin and destination + nodes, as well as the rotation and translation values. Args: - edge (igraphEdge): Used to represent an edge in the graph. It has - attributes such as `origin`, `destination`, `attrs`, and `value`. + edge (igraphEdge): Passed as a whole object, containing attributes + such as origin, destination, rt translation, and rotation euler xyz. """ origin_node = self.graph.vs.find(id=edge.origin) @@ -746,12 +778,15 @@ def insert_dsr_edge(self, org, dest): # print("ORG::", org) # self.insert_dsr_vertex(dest) """ - Modifies an existing edge in a graph based on information from two nodes, - adding new attributes for the RT translation and rotation of the edge. + Modifies an existing edge in a graph by adding new attributes representing + RT translation and rotation, and then inserts or assigns the updated edge + to the graph. Args: - org (Agent): Used to specify the origin node of the edge to be inserted. - dest (Agent): Used to represent the destination node or edge in the graph. + org (Agent): Either None or an existing node object in the graph, + representing the source node of the edge to be inserted. + dest (Agent): Used to specify the target node in the graph for the new + edge. """ if org is None: @@ -783,9 +818,9 @@ def insert_dsr_edge(self, org, dest): def draw_graph(self): """ - Clears the axis, layouts the graph using Kamada-Kawai layout algorithm, - and then plots the vertices and edges with grey lines and arrowheads - pointing towards the nodes. It also displays node names using text annotation. + Clears the axis, layouts the graph using the Kamada-Kawai algorithm, and + then plots the vertices and edges with labels and arrows. It also displays + node names. """ self.ax.clear() @@ -814,17 +849,16 @@ def draw_graph(self): def check_element_room_number(self, node_id): """ - Determines the room ID of an element with the given node ID, retrieving - the attribute value from the node's attributes and returning it if successful, - or -1 otherwise. + Determines the room number associated with a given node ID and returns it + if found, or -1 otherwise. Args: - node_id (int): Passed as an argument to the function representing a - unique identifier for an element in the graph. + node_id (str): Passed as an argument to the function, representing the + ID of a node in the graph. Returns: - int: The room ID corresponding to the given node ID when the node has - the "room_id" attribute, or -1 otherwise. + int: The room ID of a given node or -1 if an error occurs when trying + to retrieve the room ID. """ node = self.g.get_node(node_id) @@ -837,16 +871,15 @@ def check_element_room_number(self, node_id): def check_element_level(self, node_id): """ - Within the SpecificWorker class checks the "level" attribute of a node and - returns its value if found, or -1 otherwise. It also performs additional - actions related to robot position and room connections in the internal graph. + Within the SpecificWorker class determines an element's level based on an + attribute value and returns that value if found, or -1 if not. Args: - node_id (str): Used to represent the unique identifier of a node in - the graph. + node_id (str): Used to identify the node whose element level will be + checked. Returns: - int: The level of an element with the given node ID. + int: Element level of a node. """ node = self.g.get_node(node_id) @@ -894,12 +927,12 @@ def check_element_level(self, node_id): def generate_room_picture(self, room_node_id): """ - Retrieves information about a room node in a graph, specifically its ID - and the IDs of its adjacent nodes with a certain edge type, and then prints - out these values and attempts to retrieve a translation attribute. + Generates an image of a room based on its node ID, retrieves and prints + the attributes of the room node, and draws the room polygon and doors. Args: - room_node_id (str): Used to get a specific node from the graph. + room_node_id (str): Used to retrieve a Node object representing the + room node for which a picture should be generated. """ room_node = self.g.get_node(room_node_id) @@ -938,13 +971,11 @@ def generate_room_picture(self, room_node_id): def insert_current_edge(self, room_id): # Insert current edge to the room """ - Creates a new edge in the graph with the current room ID and assigns it - to the `self.g` variable, inserting or assigning the edge if it does not - already exist. + Inserts or assigns an edge into the graph, with the `room_id` as the tail + id and the `self.agent_id` as the head id. Args: - room_id (str): Provided as an argument to the function by the caller, - representing the room ID that the current edge is associated with. + room_id (str): A unique identifier for a particular room. """ current_edge = Edge(room_id, room_id, "current", self.agent_id) @@ -961,14 +992,14 @@ def update_node_att(self, id: int, attribute_names: [str]): def update_node(self, id: int, type: str): """ - Updates the state of an affordance node based on its ID and type. If the - active affordance node's state is "completed" and its active status is - false, it transitions to the "crossed" state. + Updates an affordance node's state based on its active ID, printing various + attributes and changing the node's state to "crossed" if it is completed + and not active. Args: - id (int): Representing the unique identifier for the node to be updated. - type (str): Used to indicate the type of the node being updated, which - can be either "active" or "completed". + id (int): Used to identify the node for which the affordance state + should be updated. + type (str): Used to identify the node's type. """ if id == self.affordance_node_active_id: @@ -990,14 +1021,16 @@ def update_edge(self, fr: int, to: int, type: str): # TODO: Posible problema si tocas el nodo room en la interfaz gráfica """ - Updates the information about the robot's edge when it moves to a new node - and the type of the edge is RT(room transition). + Updates an edge in the graph based on the type and values of its arguments. + If there is no current edge with the specified type, it inserts a new edge + and sets it as the current one. Args: - fr (int): The ID of the room exit door. - to (int): Passed to identify the destination node for updating the edge. - type (str): Representing the type of edge to be updated, which can be - either "RT" or "PT". + fr (int): Used to represent the ID of the current room door node that + the function operates on. + to (int): The ID of the target node to which the edge is to be updated. + type (str): Used to specify the type of edge to be updated, which can + be either "RT" or "current". """ 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: From 64393a51aa64dd52bf698c302b62dfcdcd53142a Mon Sep 17 00:00:00 2001 From: "komment-ai[bot]" <122626893+komment-ai[bot]@users.noreply.github.com> Date: Wed, 10 Jul 2024 09:56:47 +0000 Subject: [PATCH 04/10] Added comments to 37 functions across 2 files --- .komment/00000.json | 615 +++++++++--------- .komment/komment.json | 5 +- agents/g2o_agent/src/specificworker.py | 380 ++++++----- .../src/specificworker.py | 343 +++++----- 4 files changed, 687 insertions(+), 656 deletions(-) diff --git a/.komment/00000.json b/.komment/00000.json index fdd68d78..14b83b66 100644 --- a/.komment/00000.json +++ b/.komment/00000.json @@ -241,187 +241,192 @@ "path": "agents/g2o_agent/src/specificworker.py", "content": { "structured": { - "description": "A class named `G2O` that handles various graph related tasks such as adding and deleting nodes and edges, updating edge attributes, and setting RT translation and rotation values. It also defines several functions for updating node attributes, updating edges, and deleting nodes or edges. The code uses the `networkx` library to handle graphs and their operations.\n\nIn summary, the code is a class that manages graph-related tasks for an RT (Real-Time) system, including adding and removing nodes and edges, updating edge attributes, and setting RT translation and rotation values.", + "description": "A class `Robot` that handles robot movement and mapping in a graph-based environment. It uses the `networkx` library for working with graphs and provides functions for updating node attributes, creating new nodes, deleting nodes, and updating edges. The code also includes functionality for handling RT (room transition) events and storing translation and rotation information for future use. Overall, the code is focused on tracking the movement of a robot in a graph-based environment and using that information to make decisions about how to navigate the space.", "items": [ { - "id": "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb", + "id": "cedd1d2f-851b-87a9-c542-e4ba389373e2", "ancestors": [], - "description": "Is responsible for processing robot poses and creating a graph representation of the environment for a specific worker in a multi-robot system, including handling edges, vertices, odometry, and room changes.", + "description": "Performs specific tasks related to a worker robot's navigation and localization in an unknown environment, such as updating the graph, getting displacement, computing covariance matrix, visualizing G2O real-time, and more.", "attributes": [ { "name": "Period", - "type_name": "instance", - "description": "Used to specify the time interval between updates of the worker's state, allowing for more efficient processing of large datasets." + "type_name": "float", + "description": "Used to control the time interval between consecutive calls to the `update_worker` method, which is responsible for updating the robot's state based on the received data. The value of `Period` determines how often the worker will be called, with larger values resulting in slower updates but potentially reducing the load on the system." }, { "name": "agent_id", "type_name": "int", - "description": "Used as a identifier for the agent that owns the specific worker." + "description": "Used as a unique identifier for the agent's id." }, { "name": "g", - "type_name": "Graph", - "description": "Used to store the graph representation of the environment, which is updated based on the worker's observations and actions." + "type_name": "undirected", + "description": "Used to store the graph representing the environment. It contains nodes and edges that represent the obstacles, walls, and other features of the environment." }, { "name": "startup_check", "type_name": "QTimersingleShot", - "description": "Used to check if the application should quit after a certain time has elapsed since startup. It's used to implement the main loop of the worker, where the worker periodically checks if it should quit based on the current time." + "description": "Used to check for the application's quit after a certain time interval, which is 200 milliseconds in this case." }, { "name": "rt_api", - "type_name": "instance", - "description": "Used to store the RT API object, which is used for real-time data processing and transmission." + "type_name": "str", + "description": "100% sure to be a valid ROS topic name for receiving real-time data from a robotic arm." }, { "name": "inner_api", "type_name": "instance", - "description": "Used to store the inner API object of the worker, which allows for communication between the worker and the main thread." + "description": "Used to store a reference to the inner API of the worker, which allows for direct communication with the worker's inner workings." }, { "name": "odometry_node_id", "type_name": "int", - "description": "3 in this example, indicating that the node with ID 3 represents the odometry information." + "description": "Used to identify the node in the graph that corresponds to the robot's odometry information. It is used to store the odometry data in the graph and for visualization purposes." }, { "name": "odometry_queue", - "type_name": "3D", - "description": "A list of tuples containing the current pose of the robot, which is updated at each step by the worker. The queue stores the last `N` poses for each node in the graph, where `N` is a user-defined parameter." + "type_name": "3element", + "description": "Used to store the odometry information of the robot in a First-In-First-Out (FIFO) queue. It stores the advance, lateral, and angular displacement of the robot at each time step, which are computed using the G2O optimization algorithm." }, { "name": "last_odometry", - "type_name": "3D", - "description": "Used to store the last known odometry of the robot, which can be used for visualization and debugging purposes." + "type_name": "3element", + "description": "Used to store the last known odometry information of the robot, which is used in the computation of the displacement and covariance matrix." }, { "name": "g2o", - "type_name": "3D", - "description": "Used to represent the pose (position, orientation, and scale) of a robot or other object in 3D space. It is used in conjunction with the `get_covariance_matrix` method to compute the covariance matrix of the robot's position and orientation." + "type_name": "Optimizer", + "description": "Used to store the Gauss-Newton optimizer for graph matching. It is used to compute marginals \nand to update the position of vertices in the graph." }, { "name": "visualizer", - "type_name": "instance", - "description": "A reference to an instance of the `Visualizer` class, which provides a user interface for visualizing the robot's pose and other relevant data in real-time." + "type_name": "Visualizer", + "description": "Used to visualize the graph and edges in real-time during the RT algorithm execution. It provides a function to update the visualization and can be used to display the graph and edges in different formats, such as 3D or 2D." }, { "name": "odometry_noise_std_dev", "type_name": "float", - "description": "0.1 by default, which represents the standard deviation of the noise added to the odometry values during the simulation for more realistic results." + "description": "0.1 by default, representing the standard deviation of noise in the odometry measurements. It helps to control the level of random fluctuations in the robot's motion." }, { "name": "odometry_noise_angle_std_dev", "type_name": "float", - "description": "0.8 by default, which represents the standard deviation of the angular noise in the odometry measurement. It affects the optimization process by controlling the spread of the angle values in the graph." + "description": "0.8 by default, representing the standard deviation of the angle noise added to the odometry measurements for more accurate pose estimation." }, { "name": "measurement_noise_std_dev", "type_name": "float", - "description": "Used to represent the standard deviation of noise in the robot's measurements. It is used in the worker's implementation to scale the measurement values to account for the noise in the data." + "description": "Used to represent the standard deviation of the noise present in the robot's measurements. It affects how much the robot's estimate deviates from the true position." }, { "name": "last_room_id", "type_name": "int", - "description": "Used to store the last room ID seen before the agent switched rooms, which is used to handle room changes during navigation tasks." + "description": "Used to store the room ID of the last room that was processed by the worker before its initialization was changed." }, { "name": "actual_room_id", "type_name": "int", - "description": "Used to keep track of the current room id that the worker is in, it's updated when the worker moves to a new room." + "description": "Used to keep track of the current room ID that the worker is in during its navigation. It is updated whenever the worker moves from one room to another." }, { "name": "elapsed", - "type_name": "int", - "description": "Used to track the elapsed time since the worker was created, which can be used to control the worker's execution time and prevent it from running for too long." + "type_name": "float", + "description": "Used to keep track of the time elapsed since the last successful message received from the robot. It is updated every time a message is processed, and its value represents the time spent processing the message." }, { "name": "room_initialized", "type_name": "bool", - "description": "Used to track whether the robot's room has been initialized or not. It is set to False when the robot moves to a new room, and True otherwise." + "description": "Used to keep track of whether the worker has initialized the room or not. It is set to False when the worker first enters a new room, and to True when it exits that room." }, { "name": "iterations", "type_name": "int", - "description": "Used to count the number of iterations performed by the worker during its execution. It is incremented each time the worker processes a new iteration of the graph optimization problem." + "description": "0-indexed, indicating the number of iterations (or passes) that the worker has performed on the graph. It is updated each time the worker processes a new edge or node in the graph." }, { "name": "hide", "type_name": "str", - "description": "Used to indicate whether the worker should be hidden or not, defaulting to False." + "description": "Used to indicate whether the worker should be hidden or not. When set to \"True\", the worker will be hidden from the main window; otherwise, it will be displayed." }, { "name": "init_graph", - "type_name": "bool", - "description": "Used to keep track of whether the graph has been initialized or not, during the processing of the ROS bag file. It's set to True when the graph is first constructed and False when the graph is reconstructed after a change in the environment." + "type_name": "Python", + "description": "Used to indicate whether the graph has been initialized or not. It is set to `True` when the graph is first constructed and then reset to `False` when the graph is updated." }, { "name": "current_edge_set", "type_name": "bool", - "description": "Used to indicate whether the current edge set has been updated or not. It's used to track the updates of the edges in real-time." + "description": "Used to keep track of whether the current edge set has been updated with a new RT translation or rotation. It is set to True when an edge set is updated with a new RT translation or rotation, and False otherwise." }, { "name": "first_rt_set", - "type_name": "bool", - "description": "Set to `True` when the worker receives its first RT (Real-time) edge from a \"room\" node, indicating that the worker should start tracking the RT translation and rotation for this room." + "type_name": "Python", + "description": "Initialized to `True` when the agent first sets a translation and rotation for the RT task, indicating that it is the first time this has happened in the simulation." }, { "name": "translation_to_set", "type_name": "3D", - "description": "Used to store the translation of a robot to a set point, calculated based on the RT messages it receives. It is updated in the `update_edge_att` method." + "description": "Used to store the translation of the RT object to the set of corners of the room." }, { "name": "rotation_to_set", "type_name": "3D", - "description": "Used to store the rotation of the robot's end effector with respect to a set reference frame. It is updated whenever the robot's pose changes, and is used in the computation of the robot's covariance matrix." + "description": "Used to store the rotation of a robot relative to its set position." }, { "name": "room_polygon", - "type_name": "Polygon", - "description": "Used to store the polygon representation of a room in the environment. It is used for collision detection and other purposes related to navigation and mapping." + "type_name": "3D", + "description": "Used to represent the room where the robot is currently located. It stores a list of 3D vertices that define the shape of the room." }, { "name": "security_polygon", - "type_name": "3D", - "description": "Used to store the security polygon of the robot, which is a convex polytope used for collision detection and avoidance." + "type_name": "list", + "description": "Used to store a polygon representing the security area around the worker's target location. It is used to check if the worker's path intersects with any obstacles or security areas, ensuring the worker's safety during its motion." }, { "name": "initialize_g2o_graph", "type_name": "instance", - "description": "Used to initialize the graph2onedges (G2O) graph during the initialization of the worker, allowing for faster computation of marginals in the optimize method." + "description": "Responsible for initializing a Graph2O graph object to represent the environment, which is used for computing the robot's pose and motion." }, { "name": "rt_set_last_time", "type_name": "int", - "description": "Used to store the time when the first RT set was observed by the worker, for updating the translation and rotation of the robot to the set." + "description": "Used to keep track of the time since the last RT set was received. It is used to determine when to send a new RT set." }, { "name": "rt_time_min", "type_name": "float", - "description": "Defined as `self.rt_time_min = 10`. It represents the minimum time interval between two RT sets for the robot to be considered in the RT algorithm." + "description": "Used to set a minimum time interval between RT sets. It serves as a threshold for determining when a new RT set should be initiated based on the agent's movement." + }, + { + "name": "last_update_with_corners", + "type_name": "int", + "description": "Used to store the last time corners were updated. It's used in conjunction with other attributes to maintain a consistent update schedule for corners." }, { "name": "timer", "type_name": "QTimer", - "description": "Used to create a timer that triggers the worker's method to update the graph every 200 milliseconds." + "description": "Used to schedule a call to the `QApplication.instance().quit()` function every 200 milliseconds, indicating that the worker should shut down." }, { "name": "compute", "type_name": "lambda", - "description": "Called when a task is assigned to the worker. It takes the task as input, processes it using a provided function, and returns the result as output." + "description": "Used to compute the marginal probability of the worker's variables given the observed data. It takes an index `i` as input and returns a tuple containing the probability of each variable in the worker's graph given the observed data, as well as the gradient of the probability with respect to the variable's value." }, { "name": "update_node_att", - "type_name": "Python", - "description": "Used to update the attributes of a node in the graph when its ID matches the specified ID." + "type_name": "QMetaObjectAttribute", + "description": "Used to update the attributes of a node in the graph when a new attribute is received from the ROS bag." }, { "name": "update_edge", - "type_name": "update", - "description": "Called whenever an edge's attributes change. It checks if the edge is a \"room\" edge and updates the \n\"translation to set\", \"rotation to set\", or \"first RT set\" variables based on the edge's type and \nattributes." + "type_name": "str", + "description": "Used to update the edge attributes based on the edge type. It takes three parameters: `fr`, `to`, and `type`, which are the index of the current edge, the index of the next edge, and the edge type, respectively. The method updates the edge attributes based on the edge type and sets the `current_edge_set` attribute to `True`." }, { "name": "update_edge_att", "type_name": "edge", - "description": "Used to update the attributes of an edge in a graph." + "description": "Used to update the attributes of an edge in the graph." } ], "name": "SpecificWorker", @@ -433,25 +438,25 @@ "comment": null }, "item_type": "class", - "length": 404, + "length": 424, "docLength": null }, { - "id": "7d7de271-5f36-6685-a041-992c749eeb89", + "id": "ba4c99e5-6e75-fe9c-5c42-896fd963bd46", "ancestors": [ - "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" + "cedd1d2f-851b-87a9-c542-e4ba389373e2" ], - "description": "Initializes member variables and sets up event handling for signals related to the graph, nodes, edges, and attributes.", + "description": "Initializes an instance of the SpecificWorker class by setting up various components such as g2o graph, visualizer, and timers. It also performs startup checks and connects signals for node and edge updates.", "params": [ { "name": "proxy_map", "type_name": "dict", - "description": "Used to map agent id's to their corresponding DSRGraph objects." + "description": "Used to store mapping information between the original graph and the transformed graph for DSR algorithm implementation." }, { "name": "startup_check", - "type_name": "bool", - "description": "Used to determine whether to run the `startup_check` method or not when initializing the SpecificWorker object." + "type_name": "Optionalbool", + "description": "Used to check for any errors in the graph during initialization." } ], "returns": null, @@ -464,35 +469,35 @@ "comment": null }, "item_type": "method", - "length": 54, + "length": 56, "docLength": null }, { - "id": "da0a9261-8bf8-ea87-794b-f1499a4f53d3", + "id": "9f5049cf-1b33-95b0-d74c-970e8a2091b1", "ancestors": [ - "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" + "cedd1d2f-851b-87a9-c542-e4ba389373e2" ], - "description": "Updates the robot's position, orientations, and security polygon based on the odometry data and the RT graph.", + "description": "Performs RT odometry computation based on sensor data and updates the Graph2O graph with landmarks, affordances, and RT edges. It also maintains counters for valid corners and doors, and rotates the robot if necessary.", "params": [], "returns": null, "name": "compute", "location": { - "start": 121, - "insert": 123, + "start": 123, + "insert": 125, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 90, + "length": 102, "docLength": null }, { - "id": "8a63dc4d-4b58-b681-ff4d-4e5147446630", + "id": "7d619d37-ef8e-0995-9f40-680ea469e08e", "ancestors": [ - "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" + "cedd1d2f-851b-87a9-c542-e4ba389373e2" ], - "description": "Initializes a Graph-based Object Recognition (G2O) graph for a specific room by:\n\n1. Finding the corners of the room using ROS topics.\n2. Adding nominal corners to the G2O graph.\n3. Adding fixed poses to the G2O graph for the robot and doors in the room.", + "description": "Initializes a Graph2O graph for a specific robot and room by:\n\n* Extracting landmarks from the robot's point cloud\n* Calculating nominal corners based on landmarks and room geometry\n* Adding fixed poses to the g2o graph for the robot and doors in the room.", "params": [], "returns": { "type_name": "bool", @@ -500,37 +505,37 @@ }, "name": "initialize_g2o_graph", "location": { - "start": 238, - "insert": 241, + "start": 247, + "insert": 250, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 109, + "length": 115, "docLength": null }, { - "id": "bef4919b-9b49-2ea6-7e42-9773a84c6c4e", + "id": "cc39009c-98a2-8683-9e40-55a39ba52731", "ancestors": [ - "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" + "cedd1d2f-851b-87a9-c542-e4ba389373e2" ], - "description": "Calculates the displacement of an object based on its odometry data, computing the lateral displacement, forward displacement, and angular displacement using a moving window approach.", + "description": "Computes and updates the displacement values (lateral, forward, and angular) of a robot based on its odometry data, using a moving average of recent positions to smooth out the motion.", "params": [ { "name": "odometry", "type_name": "3element", - "description": "Used to store the robot's motion information at each time step, including its linear displacement, lateral displacement, and angular displacement." + "description": "An instance of the `Odometry` class, representing the robot's position and velocity over time." } ], "returns": { "type_name": "3element", - "description": "Displacement in three directions (lateral, advance and angular) calculated using the odometry data from a queue." + "description": "The lateral displacement (in meters), the forward displacement (in meters) and angular displacement (in radians)." }, "name": "get_displacement", "location": { - "start": 437, - "insert": 438, + "start": 452, + "insert": 453, "offset": " ", "indent": 8, "comment": null @@ -540,26 +545,26 @@ "docLength": null }, { - "id": "ae20cbb0-b446-7387-2246-a1505aa2b1c1", + "id": "90a11b67-7e49-1f85-f542-5b75590854cc", "ancestors": [ - "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" + "cedd1d2f-851b-87a9-c542-e4ba389373e2" ], "description": "Computes the covariance matrix for a given vertex in a graph, using an optimization algorithm to compute the marginals of the vertices and then constructing the covariance matrix.", "params": [ { "name": "vertex", - "type_name": "g2overtex", - "description": "Used to compute the covariance matrix of the vertices in the graph." + "type_name": "g2oVertex", + "description": "Used to represent a vertex in the graph." } ], "returns": { - "type_name": "tuple", - "description": "2-element, where the first element is a boolean value indicating whether the covariance matrix was computed or not and the second element is the actual covariance matrix." + "type_name": "2tuple", + "description": "A pair of a boolean result and a covariance matrix." }, "name": "get_covariance_matrix", "location": { - "start": 456, - "insert": 457, + "start": 471, + "insert": 472, "offset": " ", "indent": 8, "comment": null @@ -569,23 +574,23 @@ "docLength": null }, { - "id": "67619954-9d16-ceb2-ee47-9e0c50ce26bd", + "id": "6eecaaa5-da88-1192-224a-b19526aa0376", "ancestors": [ - "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" + "cedd1d2f-851b-87a9-c542-e4ba389373e2" ], - "description": "Within the `SpecificWorker` class loads an G2O file, visualizes its vertices and edges in 3D, and updates the visualization in real-time as the optimizer processes new measurements.", + "description": "Within the `SpecificWorker` class loads a G2O file, visualizes the vertices and edges of the graph in a 3D scatter plot, and updates the plot in real-time as new measurements are received.", "params": [ { "name": "optimizer", "type_name": "instance", - "description": "Used to load G2O files." + "description": "An object that loads G2O files and provides access to their vertices and edges for visualization." } ], "returns": null, "name": "visualize_g2o_realtime", "location": { - "start": 469, - "insert": 470, + "start": 484, + "insert": 485, "offset": " ", "indent": 8, "comment": null @@ -595,28 +600,28 @@ "docLength": null }, { - "id": "ef528e02-66ad-7da9-964d-91a42afb8717", + "id": "25fc8d71-2815-d7be-5f44-ddf8121e9654", "ancestors": [ - "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" + "cedd1d2f-851b-87a9-c542-e4ba389373e2" ], - "description": "Updates the attributes of a node in a graph, specifically the odometry node, by appending values to an odometry queue.", + "description": "Updates an attribute of a node in a graph, specifically the odometry node, by appending its current position and velocity to a queue for processing.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to represent the node ID of interest for updating attributes." + "description": "Passed as an argument to the function, representing the unique identifier for the node being updated." }, { "name": "attribute_names", "type_name": "[str]", - "description": "An array of strings representing the names of attributes to be updated on the node." + "description": "A list of names of attributes to update on the node associated with the given ID." } ], "returns": null, "name": "update_node_att", "location": { - "start": 506, - "insert": 515, + "start": 521, + "insert": 530, "offset": " ", "indent": 8, "comment": null @@ -626,28 +631,28 @@ "docLength": null }, { - "id": "fa72d1a2-e9a6-1481-7d44-8a7f242085b0", + "id": "003e6136-27e7-beb3-ab41-3f3c52ff0bb3", "ancestors": [ - "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" + "cedd1d2f-851b-87a9-c542-e4ba389373e2" ], - "description": "Updates an unknown node with the specified ID, performing different actions based on the type of node it represents.", + "description": "Updates a node's type, but only if the type is \"corner\". If it is, the function also checks if the \"room\" node has been initialized and sets the `init_graph` attribute to `True`.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to identify the node being updated." + "description": "Used to represent the unique identifier of a node." }, { "name": "type", "type_name": "str", - "description": "Used to specify the node's type, with possible values being \"corner\"." + "description": "Used to indicate the type of node being updated, specifically whether it is a corner node or not." } ], "returns": null, "name": "update_node", "location": { - "start": 522, - "insert": 532, + "start": 537, + "insert": 547, "offset": " ", "indent": 8, "comment": null @@ -657,11 +662,11 @@ "docLength": null }, { - "id": "3870a905-4bed-33bc-1444-26249501564e", + "id": "7ae77879-8a05-aa85-404f-29e9171fb57a", "ancestors": [ - "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" + "cedd1d2f-851b-87a9-c542-e4ba389373e2" ], - "description": "Deletes a node from the graph represented by the `SpecificWorker` class, setting the `room_initialized` attribute to `False`.", + "description": "Deletes a node from a list maintained by a `SpecificWorker` instance, setting a flag to indicate that the list has been modified.", "params": [ { "name": "id", @@ -672,8 +677,8 @@ "returns": null, "name": "delete_node", "location": { - "start": 534, - "insert": 535, + "start": 549, + "insert": 550, "offset": " ", "indent": 8, "comment": null @@ -683,33 +688,33 @@ "docLength": null }, { - "id": "21b579b0-e2c1-b8af-0d49-f97a5f6121e1", + "id": "244f7983-496d-92a0-dc48-2e8b20b80070", "ancestors": [ - "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" + "cedd1d2f-851b-87a9-c542-e4ba389373e2" ], - "description": "Updates the room ID and sets the current edge set to true when the type is \"current\" or \"RT\" and the source node is a room and the target node is \"Shadow\". It also sets the translation and rotation to set variables when RT is set for the first time.", + "description": "Updates the current room ID and RT translation and rotation when the agent moves from one room to another, and sets the `current_edge_set` variable to `True`.", "params": [ { "name": "fr", "type_name": "int", - "description": "Used to represent the ID of the current node being processed in the graph." + "description": "A reference to a node in the graph represented by the class instance." }, { "name": "to", "type_name": "int", - "description": "The id of the next node in the graph that the edge is pointing to." + "description": "Used as an index to access the target node of an edge in the graph." }, { "name": "type", "type_name": "str", - "description": "Used to determine which room the edge updates are for, either the current room or RT (room to Shadow)" + "description": "Used to identify the edge type, which can be either \"current\" or \"RT\"." } ], "returns": null, "name": "update_edge", "location": { - "start": 543, - "insert": 544, + "start": 558, + "insert": 559, "offset": " ", "indent": 8, "comment": null @@ -719,33 +724,33 @@ "docLength": null }, { - "id": "5a1c2562-2abe-0a91-2f4d-cda682735284", + "id": "7c04e666-9a89-a691-0247-1180243804a0", "ancestors": [ - "7f979d1a-dbf9-739d-c442-d7cd92ce4fcb" + "cedd1d2f-851b-87a9-c542-e4ba389373e2" ], - "description": "Deletes an edge from a graph with specified source and target nodes, based on the type of edge.", + "description": "Deletes an edge from a graph based on its FID (fr), TID (to), and edge type (type).", "params": [ { "name": "fr", "type_name": "int", - "description": "Given a value of 13." + "description": "Used as the index of the edge to be deleted." }, { "name": "to", "type_name": "int", - "description": "Specified as an index of an edge to be deleted." + "description": "Used to represent the destination node ID for edge deletion." }, { "name": "type", "type_name": "str", - "description": "Used to specify the edge type to be deleted." + "description": "Used to specify the edge type that should be deleted." } ], "returns": null, "name": "delete_edge", "location": { - "start": 571, - "insert": 572, + "start": 586, + "insert": 587, "offset": " ", "indent": 8, "comment": null @@ -1158,137 +1163,137 @@ "path": "agents/long_term_spatial_memory_agent/src/specificworker.py", "content": { "structured": { - "description": "An agent's internal graph and performs various operations on it, including:\n\n* Generating a room picture by iterating over the room nodes and their RT edges, and drawing a black image proportional to the room size with the room polygon and doors circled in white.\n* Inserting current edge to the room when there is no current edge and the agent's state is \"crossed\".\n* Updating node attributes and node type based on user input.\n* Deleting nodes and edges based on user input.", + "description": "an AI agent that navigates through a 3D space and interacts with objects in that space using a deep reinforcement learning algorithm. The agent maintains a graph representation of its environment and uses this graph to plan its actions and learn from its experiences. The code includes various methods for updating the agent's internal state, such as updating its node attributions, updating its node, and deleting nodes or edges from its graph. Additionally, it includes methods for generating room pictures, inserting current edges, and checking for startup. Overall, this code provides a comprehensive framework for an AI agent to navigate and interact with its environment using deep reinforcement learning.", "items": [ { - "id": "0a067d35-4dda-36b4-5c41-7f6586689420", + "id": "14d16d1c-f321-80b9-164e-7904534324da", "ancestors": [], - "description": "Performs various tasks related to a specific worker in a multi-agent system, such as checking for room occupation, updating node attributes, and generating room images. It also handles insertion and deletion of edges and nodes, and updates the state of the agent based on certain conditions.", + "description": "Manages a graph representing an agent's environment and performs various tasks related to navigation, such as updating node attributes, inserting edges, and deleting nodes or edges. It also handles startup checks and provides methods for checking the current room, element level, and room number, as well as generating a picture of the room and inserting a current edge.", "attributes": [ { "name": "Period", - "type_name": "int", - "description": "200 milliseconds, which is the time interval between updates of the worker's state." + "type_name": "str", + "description": "20 seconds long, indicating how often the worker will execute its function." }, { "name": "agent_id", - "type_name": "integerint", - "description": "Used as a unique identifier for each agent in the swarm." + "type_name": "int", + "description": "Used to store the ID of the agent that this worker represents." }, { "name": "g", "type_name": "Graph", - "description": "Used to store the graph representation of the environment that the worker is working on. It contains nodes, edges, and other attributes that are relevant for the worker's task." + "description": "Used to represent the robot's internal graph, which stores information about the \nrobot's environment, such as nodes (rooms), edges, and their attributes. The graph is \nused to model the robot's movement and interactions with its environment." }, { "name": "update_node", - "type_name": "UpdateNode", - "description": "Used to update the state of a node in the graph based on its ID, type, and affordance state." + "type_name": "int", + "description": "Used to update the state of a specific node in the graph based on its type (either \"active\" or \"crossed\"). It takes an argument `type`, which can be either \"active\" or \"crossed\", and updates the corresponding attribute of the node with the given ID." }, { "name": "update_edge", - "type_name": "str", - "description": "Used to set a new current edge in the graph when there is no existing current edge and the room node exists." + "type_name": "Edge", + "description": "Used to set a new current edge between two nodes when there is no existing current edge and the room node exists." }, { "name": "startup_check", "type_name": "QTimersingleShot", - "description": "Used to start the application's shutdown process after a certain period of time (200 milliseconds) since the worker was created." + "description": "Set to call the `QApplication.instance().quit` function after 200 milliseconds of starting up the worker, indicating that the worker has finished its initialization and is ready to process tasks." }, { "name": "rt_api", "type_name": "str", - "description": "Used for handling robot actions, specifically RT (Remote Transport) actions. It provides a way to interact with the robot's internal API to perform actions such as moving or manipulating objects." + "description": "Used to store the API for the robot's RT (Real-Time) communication. It is used to send commands and receive updates from the robot in real-time." }, { "name": "inner_api", - "type_name": "API", - "description": "Used to access the internal API of the agent, allowing the worker to interact with the agent's internal state and perform actions such as generating room pictures or updating node attributes." + "type_name": "internal", + "description": "Used to access the agent's inner API for retrieving information such as room coordinates, door connections, and robot position." }, { "name": "robot_name", "type_name": "str", - "description": "Used to store the name of the robot agent in the environment." + "description": "Used to store the name of the robot. It is set during the construction of the worker object and can be accessed later for various purposes such as communication with other agents or saving in a graph." }, { "name": "robot_id", "type_name": "int", - "description": "Used to store the ID of the robot node in the graph. It is used to identify the current room and for RT edges." + "description": "Used to represent the ID of the robot that is currently in the room." }, { "name": "last_robot_pose", "type_name": "Pose", - "description": "Used to store the last known pose of the robot in the environment, which is used for motion planning and collision detection." + "description": "Updated each time the worker moves, storing the last known pose of the robot before any movement occurred." }, { "name": "robot_exit_pose", "type_name": "Pose", - "description": "Used to store the pose of the robot when it exits a room." + "description": "Used to represent the pose of a robot when it exits a room. It is used in the `insert_current_edge` method to determine when a new edge should be inserted as current." }, { "name": "state", "type_name": "str", - "description": "Used to store the current state of the worker, which can be either \"idle\", \"running\", or \"crossed\"." + "description": "Used to store the current state of the agent, which can be either \"idle\", \"crossed\", or \"completed\"." }, { "name": "affordance_node_active_id", "type_name": "int", - "description": "Used to store the ID of the node representing the affordance that the robot is currently interacting with." + "description": "Used to store the ID of the affordance node that is currently active or being processed by the worker." }, { "name": "exit_door_id", "type_name": "int", - "description": "0 by default, indicating the door through which the robot exits a room when it moves to another room." + "description": "12 by default, indicating the door ID of the room that the worker exits from when it finishes its task." }, { "name": "room_exit_door_id", "type_name": "int", - "description": "31 by default, representing the door ID of the room that the worker is exiting." + "description": "4 by default, representing the ID of the door that marks the exit of a room in the environment." }, { "name": "enter_room_node_id", "type_name": "int", - "description": "Used to store the ID of the room node that the worker has entered. It is used to check if the worker has reached the goal state by comparing it with the ID of the goal room node." + "description": "Used to store the ID of the room node that the worker is currently inside." }, { "name": "graph", "type_name": "Graph", - "description": "Used to represent the graph of nodes and edges in the environment, which is updated based on the worker's actions and observations." + "description": "Used to store the robot's internal graph, which represents the state of the environment and the robot's actions." }, { "name": "vertex_size", "type_name": "int", - "description": "16 by default, indicating the size of each vertex in the graph." + "description": "16 by default, indicating the size of each vertex in pixels. It can be used to adjust the visual representation of the graph in the worker's internal API." }, { "name": "not_required_attrs", "type_name": "list", - "description": "Used to store a list of node attributes that are not required for the worker's functionality. It helps in avoiding unnecessary computation and improving performance by skipping the retrieval of these attributes during node traversal." + "description": "Used to specify a list of attributes that are not required for the worker's functionality. These are attributes that can be omitted without affecting the worker's behavior or performance." }, { "name": "long_term_graph", "type_name": "Graph", - "description": "Used to store the long-term graph of the environment, which represents the entire state of the environment over time." + "description": "Used to store the internal graph of the agent over time, allowing the worker to keep track of the agent's long-term memory and reasoning." }, { "name": "global_map", - "type_name": "npndarray", - "description": "Used to store a map of the environment, which is used for navigation and other purposes." + "type_name": "dict", + "description": "Used to store the global map image that the worker uses for navigation. It is initialized with a blank dictionary when the worker starts and can be modified by calling methods such as `insert_current_edge` and `update_node`." }, { "name": "insert_current_edge", "type_name": "Edge", - "description": "Used to insert a new edge into the graph with the current room as the source node and the robot node as the destination node, indicating that the robot has entered the current room." + "description": "Used to insert a new edge in the graph with the current room as the origin and the destination room, signaling that the robot has entered the room." }, { "name": "timer", "type_name": "QTimer", - "description": "Used to schedule a call to the `startup_check()` function after 200 milliseconds of starting the application, which signals the agent to start its work." + "description": "Used to schedule a callback function to be called after a certain time (in milliseconds) has passed." }, { "name": "compute", - "type_name": "instance", - "description": "Used to compute the cost of the robot moving from its current state to a new state. It takes into account factors such as distance, obstacles, and door states." + "type_name": "str", + "description": "Used to hold the worker's computation result" } ], "name": "SpecificWorker", @@ -1300,25 +1305,25 @@ "comment": null }, "item_type": "class", - "length": 533, + "length": 537, "docLength": null }, { - "id": "8f7fe684-295f-f8b1-c94d-1c5ce453de90", + "id": "f5c0bd08-589a-4b8b-a14e-7fd68022b8be", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Initializes an instance of the SpecificWorker class by setting up various graph structures, defining variables, and connecting signals for updating node and edge positions.", + "description": "Initializes an object of the `SpecificWorker` class, setting properties and connecting signals for updating node and edge positions in a graph representing a spatial memory environment.", "params": [ { "name": "proxy_map", "type_name": "igraphGraph", - "description": "Used to specify the graph that represents the environment for the worker agent." + "description": "Passed to the superclass's constructor. It represents the graph that the SpecificWorker class will work on." }, { "name": "startup_check", "type_name": "bool", - "description": "Used to check if the agent has already started. If set to `False`, it will call the `startup_check` method instead, which initializes the internal state of the agent." + "description": "Used to determine whether to run a check during startup, which involves checking for validity of graph edges." } ], "returns": null, @@ -1335,16 +1340,16 @@ "docLength": null }, { - "id": "b7663dc2-9b1f-eaaf-a64b-a95a80099f99", + "id": "f9acbd22-c01b-23b7-6f4f-b4fa40fec045", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Sets parameters and modifies the state of a Room object by removing a self-edge, storing an exit door ID, setting attribute values for doors, and reading an entrance door node to add an other_side_door attribute with the name of the exit door in the new room.", + "description": "Sets the parameters for a worker instance, specifically removing a self-edge from a room and updating various attributes related to doors.", "params": [ { "name": "params", "type_name": "object", - "description": "Passed in from outside the function to be processed or modified." + "description": "Used to set various attributes of the room and its doors." } ], "returns": { @@ -1364,11 +1369,11 @@ "docLength": null }, { - "id": "4b952eb9-acba-209b-de45-48fe3b8d2de5", + "id": "9f58c232-e34f-ca91-4848-7ab916c1e754", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Determines the state of a worker based on the current state of the graph and performs appropriate actions according to the state.", + "description": "Determines the current state of a worker and calls the appropriate method to update its state based on the current state machine.", "params": [], "returns": null, "name": "compute", @@ -1384,11 +1389,11 @@ "docLength": null }, { - "id": "59d68e36-a969-1390-a64b-66a3c5f1207a", + "id": "8089bfc6-1aec-89b3-ff4c-2221ef94ad24", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Generates high-quality summaries of Java code given to it, checks if any \"current\" edge exists, and if not, associates doors based on their proximity.", + "description": "Determines the robot's affordance in the current room and calculates the pose of the door to reach the center of a new room. It also associates doors, updates node attributes, and changes the state of the worker.", "params": [], "returns": null, "name": "idle", @@ -1400,21 +1405,21 @@ "comment": null }, "item_type": "method", - "length": 71, + "length": 75, "docLength": null }, { - "id": "73ce6331-230e-b191-4445-a4e68e4cfb3a", + "id": "59952246-8d88-ffad-ea44-e45c226538f7", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Manages the node and edge properties for a worker class called `SpecificWorker`, which is an extension of `GenericWorker`. It retrieves the affordance node, checks if it has a parent, and updates the exit door ID node with the connected room name.", + "description": "Determines the exit door ID for a specific room based on an active affordance node and updates the state of the worker accordingly.", "params": [], "returns": null, "name": "crossed", "location": { - "start": 264, - "insert": 266, + "start": 268, + "insert": 270, "offset": " ", "indent": 8, "comment": null @@ -1424,17 +1429,17 @@ "docLength": null }, { - "id": "b038074f-0d1d-37bb-7949-1863b4f1a32a", + "id": "3d97375c-3e55-c7b9-1d40-4fee711096bc", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Determines if there are any rooms other than the current room that the worker is in, and if so, sets the enter room node ID and inserts an edge to it. It then sets the state to \"initializing doors\" and prints a message.", + "description": "Determines and saves the ID of the first non-exiting room node in the graph, and sets the state to \"initializing doors\".", "params": [], "returns": null, "name": "initializing_room", "location": { - "start": 284, - "insert": 287, + "start": 288, + "insert": 291, "offset": " ", "indent": 8, "comment": null @@ -1444,17 +1449,17 @@ "docLength": null }, { - "id": "40ac65e2-76b8-2b98-194e-80c545d8fd7a", + "id": "81c984ff-ff5e-afab-5d46-5440953dbe7f", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "In the `SpecificWorker` class is responsible for handling the scenario where the robot needs to move to another room through a door, and there is no direct path from the current room to the desired room. It traverses the graph to find the shortest path to the desired room, updates the node's parent attribute, and inserts a new edge in the graph to connect the robot to the desired room.", + "description": "Given to an instance of the SpecificWorker class, identifies the room that the robot is currently located in and returns relevant information about it.", "params": [], "returns": null, "name": "known_room", "location": { - "start": 298, - "insert": 300, + "start": 302, + "insert": 304, "offset": " ", "indent": 8, "comment": null @@ -1464,17 +1469,17 @@ "docLength": null }, { - "id": "97fb42e6-e393-a18f-0d40-f962134e20f1", + "id": "1a1e3b3c-b20c-c79f-5c42-f0dbaa2e4333", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Identifies the doors connected to the current room and updates their attributes with the names of the rooms they lead to, also associating them through a pair of tuples.", + "description": "Identifies and updates the connections between doors in a graph, based on their types and attributes. It associates doors with each other and updates node attributes to reflect their relationships.", "params": [], "returns": null, "name": "initializing_doors", "location": { - "start": 392, - "insert": 394, + "start": 396, + "insert": 398, "offset": " ", "indent": 8, "comment": null @@ -1484,28 +1489,28 @@ "docLength": null }, { - "id": "169b50a4-d8c6-22be-b548-e904c2d484da", + "id": "517f46f2-3ff6-5e9f-4243-ba8f95693b69", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Connects two doors in a graph by adding an edge between their nodes and updating the \"other side door name\" and \"connected room name\" attributes of each door node.", + "description": "Adds an edge between two nodes in a graph, linking them as doors to each other's rooms. It also assigns the names of the other door and connected room to each node.", "params": [ { "name": "door_1", - "type_name": "igraphNode", - "description": "A reference to one of the doors in the graph, which is used to establish a connection between two rooms." + "type_name": "str", + "description": "2-element list representing a door node name and its associated room name in an igraph graph." }, { "name": "door_2", - "type_name": "str", - "description": "2-dimensional list containing the name of the door and its room information." + "type_name": "IgraphVertex", + "description": "Represented as a list containing the room name and the adjacent room name." } ], "returns": null, "name": "associate_doors", "location": { - "start": 431, - "insert": 433, + "start": 435, + "insert": 437, "offset": " ", "indent": 8, "comment": null @@ -1515,17 +1520,17 @@ "docLength": null }, { - "id": "7dba6d19-0ac9-11a8-5f4d-7ab151351297", + "id": "8b48d263-c9fe-bc96-5749-1000b78b7843", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Stores the graph data in a file called \"graph.pkl\" when a room node is found in the igraph data structure.", + "description": "Stores the graph data in a pickle file \"graph.pkl\".", "params": [], "returns": null, "name": "store_graph", "location": { - "start": 450, - "insert": 451, + "start": 454, + "insert": 455, "offset": " ", "indent": 8, "comment": null @@ -1535,17 +1540,17 @@ "docLength": null }, { - "id": "6003f927-ac68-b2ba-564e-ba42175519a0", + "id": "5e9c08ef-5fbc-0d93-b441-f4b177f7bd6a", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Removes edges and nodes from the graph based on certain room numbers, updates the long-term graph, and changes the state to \"idle\".", + "description": "Removes nodes from the graph that have no incoming or outgoing edges, and shadows nodes with specific room numbers.", "params": [], "returns": null, "name": "removing", "location": { - "start": 464, - "insert": 466, + "start": 468, + "insert": 470, "offset": " ", "indent": 8, "comment": null @@ -1555,23 +1560,23 @@ "docLength": null }, { - "id": "1ab47154-ba0f-2487-4144-57a467c74f40", + "id": "1e568e41-1cae-638e-c944-d3abc3ff3edd", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Within the SpecificWorker class recursively traverses the graph by following RT edges from the starting node until reaching the robot's ID or a maximum depth limit is reached. It inserts vertices and edges into an igraph handle for each iteration.", + "description": "Traverses the graph represented by the `Igraph` object `g`. It starts at a specified node (`node_id`) and explores its RT children, then recursively traverses the graph to the destination of each RT child.", "params": [ { "name": "node_id", "type_name": "int", - "description": "The unique identifier of the node to be traversed." + "description": "Represented by the `g.get_node()` method, which retrieves a node from the graph based on its ID." } ], "returns": null, "name": "traverse_graph", "location": { - "start": 490, - "insert": 492, + "start": 494, + "insert": 496, "offset": " ", "indent": 8, "comment": null @@ -1581,23 +1586,23 @@ "docLength": null }, { - "id": "9941450b-8dc1-aaac-2849-a5d941339841", + "id": "6bb1abc3-e4f5-a8be-8d4f-df44de6d1f92", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Navigates through the graph by starting at a given vertex, identifying its successors, and inserting vertices and edges to preserve the DSR property of the graph. It then recursively traverses the successor vertices until no more updates are needed.", + "description": "Traverses the graph and inserts DSR vertices and edges based on certain conditions.", "params": [ { "name": "node", "type_name": "igraphVertex", - "description": "Representing an existing vertex in the graph." + "description": "An instance of vertex in a graph representing a node with its attributes such as index, name, room ID, and level." } ], "returns": null, "name": "traverse_igraph", "location": { - "start": 500, - "insert": 501, + "start": 504, + "insert": 505, "offset": " ", "indent": 8, "comment": null @@ -1607,23 +1612,23 @@ "docLength": null }, { - "id": "225fa64e-4fe1-6387-0640-047b650806c6", + "id": "6d280869-b88e-6d8b-994f-d1e0dd1cc3de", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Adds a new vertex to an igraph graph, handling various attributes and edges based on their types and values.", + "description": "Adds a new vertex to an igraph graph based on input node data and updates the graph's vertices with the corresponding attributes.", "params": [ { "name": "node", "type_name": "igraphNode", - "description": "Passed to insert a new vertex into an existing graph." + "description": "Used to represent a vertex to be added to an igraph graph. It contains information such as the vertex name, ID, and type." } ], "returns": null, "name": "insert_igraph_vertex", "location": { - "start": 513, - "insert": 514, + "start": 517, + "insert": 518, "offset": " ", "indent": 8, "comment": null @@ -1633,28 +1638,28 @@ "docLength": null }, { - "id": "776dd3e6-e846-cd8c-9b46-91072201c62e", + "id": "8af24c89-c672-9d98-324b-58e820167ab8", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Inserts a new node into a graph, updating the node's attributes and parent relationship.", + "description": "Inserts a new node into a graph, setting its parent node and attributes based on the provided node object.", "params": [ { "name": "parent_name", "type_name": "str", - "description": "Required, which refers to the name of a vertex in the graph that will serve as the parent for the new node being inserted." + "description": "Used to specify the name of the parent node that will hold the newly created node as a child." }, { "name": "node", - "type_name": "Node", - "description": "Passed as an instance of the Node class representing a vertex to be inserted into the graph." + "type_name": "Python", + "description": "Passed as an instance of class `Node`." } ], "returns": null, "name": "insert_dsr_vertex", "location": { - "start": 548, - "insert": 550, + "start": 552, + "insert": 554, "offset": " ", "indent": 8, "comment": null @@ -1664,23 +1669,23 @@ "docLength": null }, { - "id": "cb8df5c5-bbfa-d99a-604f-44caea2e4e59", + "id": "74eb41e5-e103-40bc-1e4f-dbf64f47284b", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Adds an edge to an igraph object, specifying the origin and destination nodes, as well as the rotation and translation values.", + "description": "Adds an edge to an igraph object, providing the origin and destination nodes and their respective attributes (translation and rotation) for the edge.", "params": [ { "name": "edge", "type_name": "igraphEdge", - "description": "Passed as a whole object, containing attributes such as origin, destination, rt translation, and rotation euler xyz." + "description": "An object containing information about an edge to be added to an igraph graph, including its origin and destination nodes, as well as rotation and translation attributes." } ], "returns": null, "name": "insert_igraph_edge", "location": { - "start": 562, - "insert": 565, + "start": 566, + "insert": 569, "offset": " ", "indent": 8, "comment": null @@ -1690,28 +1695,28 @@ "docLength": null }, { - "id": "c9505cbd-5885-1895-b749-53af73e7aa35", + "id": "067fc9ce-6d2c-50b3-9642-530b595fd5e7", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Modifies an existing edge in a graph by adding new attributes representing RT translation and rotation, and then inserts or assigns the updated edge to the graph.", + "description": "Modifies an existing edge in a graph by adding new attributes \"rt_translation\" and \"rt_rotation_euler_xyz\". It then inserts or assigns the modified edge into the graph.", "params": [ { "name": "org", "type_name": "Agent", - "description": "Either None or an existing node object in the graph, representing the source node of the edge to be inserted." + "description": "Represented as a Node object in the graph, containing information such as id, name, and rotation." }, { "name": "dest", "type_name": "Agent", - "description": "Used to specify the target node in the graph for the new edge." + "description": "Used to represent the target node or edge of the graph, which will be updated with the new RT value and rotation information." } ], "returns": null, "name": "insert_dsr_edge", "location": { - "start": 574, - "insert": 577, + "start": 578, + "insert": 581, "offset": " ", "indent": 8, "comment": null @@ -1721,17 +1726,17 @@ "docLength": null }, { - "id": "bd3d9b86-fbfb-58a0-f047-f251b71e65aa", + "id": "00c1cc7c-1774-8f91-744a-08d2a9dcebe7", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Clears the axis, layouts the graph using the Kamada-Kawai algorithm, and then plots the vertices and edges with labels and arrows. It also displays node names.", + "description": "Clears the axis, layouts the graph using the Kamada-Kawai algorithm, and plots arrows for edges. It also adds node labels and sets xy limits.", "params": [], "returns": null, "name": "draw_graph", "location": { - "start": 604, - "insert": 605, + "start": 608, + "insert": 609, "offset": " ", "indent": 8, "comment": null @@ -1741,26 +1746,26 @@ "docLength": null }, { - "id": "1c5969a2-d7a8-9fb8-154b-05151508cdac", + "id": "0c45547a-d643-a3a4-6841-fb69f9b9d7ac", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Determines the room number associated with a given node ID and returns it if found, or -1 otherwise.", + "description": "Retrieves the room ID associated with a given node ID through attribute retrieval and returns it if successful, or -1 otherwise.", "params": [ { "name": "node_id", "type_name": "str", - "description": "Passed as an argument to the function, representing the ID of a node in the graph." + "description": "Used to identify the node whose room number needs to be retrieved." } ], "returns": { "type_name": "int", - "description": "The room ID of a given node or -1 if an error occurs when trying to retrieve the room ID." + "description": "The room ID of a given node's element or -1 if an error occurs when trying to retrieve the room ID." }, "name": "check_element_room_number", "location": { - "start": 629, - "insert": 630, + "start": 633, + "insert": 634, "offset": " ", "indent": 8, "comment": null @@ -1770,26 +1775,26 @@ "docLength": null }, { - "id": "8e705cf8-c7fa-4f9f-854a-5426294aa78f", + "id": "8b8d5f6b-ab49-e8ac-264d-78917524c774", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Within the SpecificWorker class determines an element's level based on an attribute value and returns that value if found, or -1 if not.", + "description": "Within SpecificWorker, a subclass of GenericWorker, retrieves the element level attribute from a given node and returns its value if found, or -1 otherwise. It also checks the robot position and adjusts door connections in the agent's internal graph if necessary.", "params": [ { "name": "node_id", "type_name": "str", - "description": "Used to identify the node whose element level will be checked." + "description": "Used as an identifier for a specific node in the graph." } ], "returns": { - "type_name": "int", - "description": "Element level of a node." + "type_name": "integerints", + "description": "The element level of the node with the given `node_id`." }, "name": "check_element_level", "location": { - "start": 638, - "insert": 639, + "start": 642, + "insert": 643, "offset": " ", "indent": 8, "comment": null @@ -1799,23 +1804,23 @@ "docLength": null }, { - "id": "8f9ba2ca-0a1e-0386-d74e-d1179313cade", + "id": "9cc9cde0-a537-cbb5-f946-cafc99280a81", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Generates an image of a room based on its node ID, retrieves and prints the attributes of the room node, and draws the room polygon and doors.", + "description": "Within the `SpecificWorker` class retrieves and processes data related to a specific room, including its ID, edges with a certain type, and translation attribute. It also draws the room polygon and doors.", "params": [ { "name": "room_node_id", "type_name": "str", - "description": "Used to retrieve a Node object representing the room node for which a picture should be generated." + "description": "Used to identify the node representing the room for which a picture needs to be generated." } ], "returns": null, "name": "generate_room_picture", "location": { - "start": 681, - "insert": 683, + "start": 685, + "insert": 687, "offset": " ", "indent": 8, "comment": null @@ -1825,23 +1830,23 @@ "docLength": null }, { - "id": "097b5487-3516-2698-aa47-fa49c84dc627", + "id": "a1d95887-ac1d-d1ae-5e4c-e5a373c17054", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Inserts or assigns an edge into the graph, with the `room_id` as the tail id and the `self.agent_id` as the head id.", + "description": "Inserts or assigns an edge in the graph with the given `room_id` as its tail and the current agent's ID as its head.", "params": [ { "name": "room_id", "type_name": "str", - "description": "A unique identifier for a particular room." + "description": "Passed as an argument to the function, representing the ID of the current room that the agent is located in." } ], "returns": null, "name": "insert_current_edge", "location": { - "start": 716, - "insert": 718, + "start": 720, + "insert": 722, "offset": " ", "indent": 8, "comment": null @@ -1851,28 +1856,28 @@ "docLength": null }, { - "id": "07b3bda5-b24d-fa91-524d-c53e1ada98da", + "id": "23efec40-805a-b496-934d-2914ec7c005d", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Updates an affordance node's state based on its active ID, printing various attributes and changing the node's state to \"crossed\" if it is completed and not active.", + "description": "Updates the state of an affordance node based on its ID and type. If the active affordance node's state is \"completed\" and its active status is false, it transitions to the \"crossed\" state.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to identify the node for which the affordance state should be updated." + "description": "Passed as an argument representing the unique identifier of the node being updated." }, { "name": "type", "type_name": "str", - "description": "Used to identify the node's type." + "description": "Passed as an argument to the function, indicating the type of node being updated." } ], "returns": null, "name": "update_node", "location": { - "start": 730, - "insert": 731, + "start": 734, + "insert": 735, "offset": " ", "indent": 8, "comment": null @@ -1882,33 +1887,33 @@ "docLength": null }, { - "id": "67cdf7cd-b880-0684-484f-a4a4089a786b", + "id": "d05c52b1-a4de-66a0-d446-982a037f8152", "ancestors": [ - "0a067d35-4dda-36b4-5c41-7f6586689420" + "14d16d1c-f321-80b9-164e-7904534324da" ], - "description": "Updates an edge in the graph based on the type and values of its arguments. If there is no current edge with the specified type, it inserts a new edge and sets it as the current one.", + "description": "Determines if there is a current edge connecting two nodes in a graph based on specific conditions and updates the current edge if necessary.", "params": [ { "name": "fr", "type_name": "int", - "description": "Used to represent the ID of the current room door node that the function operates on." + "description": "A reference to a specific node in the graph." }, { "name": "to", "type_name": "int", - "description": "The ID of the target node to which the edge is to be updated." + "description": "Referred to as the ID of a node in the graph." }, { "name": "type", "type_name": "str", - "description": "Used to specify the type of edge to be updated, which can be either \"RT\" or \"current\"." + "description": "Set to \"RT\". It represents the type of edge being updated." } ], "returns": null, "name": "update_edge", "location": { - "start": 745, - "insert": 749, + "start": 749, + "insert": 753, "offset": " ", "indent": 8, "comment": null diff --git a/.komment/komment.json b/.komment/komment.json index 73ea794c..a3dd6f90 100644 --- a/.komment/komment.json +++ b/.komment/komment.json @@ -1,12 +1,13 @@ { "meta": { "version": "1", - "updated_at": "2024-07-08T15:05:30.287Z", + "updated_at": "2024-07-10T09:56:40.559Z", "created_at": "2024-07-03T15:48:27.349Z", "pipelines": [ "50c96b7c-ad3b-4b93-ae19-7f6fbd5637f7", "9e4d0253-62f2-4920-b2d3-607cb4cf3291", - "510e9758-a82c-441e-9653-dc071bb409eb" + "510e9758-a82c-441e-9653-dc071bb409eb", + "1f636eb0-7f1e-4d1d-b462-6d1eff3533cb" ] }, "lookup": [ diff --git a/agents/g2o_agent/src/specificworker.py b/agents/g2o_agent/src/specificworker.py index 2dceb299..5351be44 100644 --- a/agents/g2o_agent/src/specificworker.py +++ b/agents/g2o_agent/src/specificworker.py @@ -49,122 +49,133 @@ class SpecificWorker(GenericWorker): """ - Is responsible for processing robot poses and creating a graph representation - of the environment for a specific worker in a multi-robot system, including - handling edges, vertices, odometry, and room changes. + Performs specific tasks related to a worker robot's navigation and localization + in an unknown environment, such as updating the graph, getting displacement, + computing covariance matrix, visualizing G2O real-time, and more. Attributes: - Period (instance): Used to specify the time interval between updates of - the worker's state, allowing for more efficient processing of large datasets. - agent_id (int): Used as a identifier for the agent that owns the specific - worker. - g (Graph): Used to store the graph representation of the environment, which - is updated based on the worker's observations and actions. - startup_check (QTimersingleShot): Used to check if the application should - quit after a certain time has elapsed since startup. It's used to - implement the main loop of the worker, where the worker periodically - checks if it should quit based on the current time. - rt_api (instance): Used to store the RT API object, which is used for - real-time data processing and transmission. - inner_api (instance): Used to store the inner API object of the worker, - which allows for communication between the worker and the main thread. - odometry_node_id (int): 3 in this example, indicating that the node with - ID 3 represents the odometry information. - odometry_queue (3D): A list of tuples containing the current pose of the - robot, which is updated at each step by the worker. The queue stores - the last `N` poses for each node in the graph, where `N` is a user-defined - parameter. - last_odometry (3D): Used to store the last known odometry of the robot, - which can be used for visualization and debugging purposes. - g2o (3D): Used to represent the pose (position, orientation, and scale) - of a robot or other object in 3D space. It is used in conjunction with - the `get_covariance_matrix` method to compute the covariance matrix - of the robot's position and orientation. - visualizer (instance): A reference to an instance of the `Visualizer` - class, which provides a user interface for visualizing the robot's - pose and other relevant data in real-time. - odometry_noise_std_dev (float): 0.1 by default, which represents the - standard deviation of the noise added to the odometry values during - the simulation for more realistic results. - odometry_noise_angle_std_dev (float): 0.8 by default, which represents the - standard deviation of the angular noise in the odometry measurement. - It affects the optimization process by controlling the spread of the - angle values in the graph. - measurement_noise_std_dev (float): Used to represent the standard deviation - of noise in the robot's measurements. It is used in the worker's - implementation to scale the measurement values to account for the noise - in the data. - last_room_id (int): Used to store the last room ID seen before the agent - switched rooms, which is used to handle room changes during navigation - tasks. - actual_room_id (int): Used to keep track of the current room id that the - worker is in, it's updated when the worker moves to a new room. - elapsed (int): Used to track the elapsed time since the worker was created, - which can be used to control the worker's execution time and prevent - it from running for too long. - room_initialized (bool): Used to track whether the robot's room has been - initialized or not. It is set to False when the robot moves to a new - room, and True otherwise. - iterations (int): Used to count the number of iterations performed by the - worker during its execution. It is incremented each time the worker - processes a new iteration of the graph optimization problem. - hide (str): Used to indicate whether the worker should be hidden or not, - defaulting to False. - init_graph (bool): Used to keep track of whether the graph has been - initialized or not, during the processing of the ROS bag file. It's - set to True when the graph is first constructed and False when the - graph is reconstructed after a change in the environment. - current_edge_set (bool): Used to indicate whether the current edge set has - been updated or not. It's used to track the updates of the edges in real-time. - first_rt_set (bool): Set to `True` when the worker receives its first RT - (Real-time) edge from a "room" node, indicating that the worker should - start tracking the RT translation and rotation for this room. - translation_to_set (3D): Used to store the translation of a robot to a set - point, calculated based on the RT messages it receives. It is updated - in the `update_edge_att` method. - rotation_to_set (3D): Used to store the rotation of the robot's end effector - with respect to a set reference frame. It is updated whenever the - robot's pose changes, and is used in the computation of the robot's + Period (float): Used to control the time interval between consecutive calls + to the `update_worker` method, which is responsible for updating the + robot's state based on the received data. The value of `Period` + determines how often the worker will be called, with larger values + resulting in slower updates but potentially reducing the load on the + system. + agent_id (int): Used as a unique identifier for the agent's id. + g (undirected): Used to store the graph representing the environment. It + contains nodes and edges that represent the obstacles, walls, and other + features of the environment. + startup_check (QTimersingleShot): Used to check for the application's quit + after a certain time interval, which is 200 milliseconds in this case. + rt_api (str): 100% sure to be a valid ROS topic name for receiving real-time + data from a robotic arm. + inner_api (instance): Used to store a reference to the inner API of the + worker, which allows for direct communication with the worker's inner + workings. + odometry_node_id (int): Used to identify the node in the graph that + corresponds to the robot's odometry information. It is used to store + the odometry data in the graph and for visualization purposes. + odometry_queue (3element): Used to store the odometry information of the + robot in a First-In-First-Out (FIFO) queue. It stores the advance, + lateral, and angular displacement of the robot at each time step, which + are computed using the G2O optimization algorithm. + last_odometry (3element): Used to store the last known odometry information + of the robot, which is used in the computation of the displacement and covariance matrix. - room_polygon (Polygon): Used to store the polygon representation of a room - in the environment. It is used for collision detection and other - purposes related to navigation and mapping. - security_polygon (3D): Used to store the security polygon of the robot, - which is a convex polytope used for collision detection and avoidance. - initialize_g2o_graph (instance): Used to initialize the graph2onedges (G2O) - graph during the initialization of the worker, allowing for faster - computation of marginals in the optimize method. - rt_set_last_time (int): Used to store the time when the first RT set was - observed by the worker, for updating the translation and rotation of - the robot to the set. - rt_time_min (float): Defined as `self.rt_time_min = 10`. It represents the - minimum time interval between two RT sets for the robot to be considered - in the RT algorithm. - timer (QTimer): Used to create a timer that triggers the worker's method - to update the graph every 200 milliseconds. - compute (lambda): Called when a task is assigned to the worker. It takes - the task as input, processes it using a provided function, and returns - the result as output. - update_node_att (Python): Used to update the attributes of a node in the - graph when its ID matches the specified ID. - update_edge (update): Called whenever an edge's attributes change. It - checks if the edge is a "room" edge and updates the - "translation to set", "rotation to set", or "first RT set" variables - based on the edge's type and - attributes. - update_edge_att (edge): Used to update the attributes of an edge in a graph. + g2o (Optimizer): Used to store the Gauss-Newton optimizer for graph matching. + It is used to compute marginals + and to update the position of vertices in the graph. + visualizer (Visualizer): Used to visualize the graph and edges in real-time + during the RT algorithm execution. It provides a function to update + the visualization and can be used to display the graph and edges in + different formats, such as 3D or 2D. + odometry_noise_std_dev (float): 0.1 by default, representing the standard + deviation of noise in the odometry measurements. It helps to control + the level of random fluctuations in the robot's motion. + odometry_noise_angle_std_dev (float): 0.8 by default, representing the + standard deviation of the angle noise added to the odometry measurements + for more accurate pose estimation. + measurement_noise_std_dev (float): Used to represent the standard deviation + of the noise present in the robot's measurements. It affects how much + the robot's estimate deviates from the true position. + last_room_id (int): Used to store the room ID of the last room that was + processed by the worker before its initialization was changed. + actual_room_id (int): Used to keep track of the current room ID that the + worker is in during its navigation. It is updated whenever the worker + moves from one room to another. + elapsed (float): Used to keep track of the time elapsed since the last + successful message received from the robot. It is updated every time + a message is processed, and its value represents the time spent + processing the message. + room_initialized (bool): Used to keep track of whether the worker has + initialized the room or not. It is set to False when the worker first + enters a new room, and to True when it exits that room. + iterations (int): 0-indexed, indicating the number of iterations (or passes) + that the worker has performed on the graph. It is updated each time + the worker processes a new edge or node in the graph. + hide (str): Used to indicate whether the worker should be hidden or not. + When set to "True", the worker will be hidden from the main window; + otherwise, it will be displayed. + init_graph (Python): Used to indicate whether the graph has been initialized + or not. It is set to `True` when the graph is first constructed and + then reset to `False` when the graph is updated. + current_edge_set (bool): Used to keep track of whether the current edge + set has been updated with a new RT translation or rotation. It is set + to True when an edge set is updated with a new RT translation or + rotation, and False otherwise. + first_rt_set (Python): Initialized to `True` when the agent first sets a + translation and rotation for the RT task, indicating that it is the + first time this has happened in the simulation. + translation_to_set (3D): Used to store the translation of the RT object + to the set of corners of the room. + rotation_to_set (3D): Used to store the rotation of a robot relative to + its set position. + room_polygon (3D): Used to represent the room where the robot is currently + located. It stores a list of 3D vertices that define the shape of the + room. + security_polygon (list): Used to store a polygon representing the security + area around the worker's target location. It is used to check if the + worker's path intersects with any obstacles or security areas, ensuring + the worker's safety during its motion. + initialize_g2o_graph (instance): Responsible for initializing a Graph2O + graph object to represent the environment, which is used for computing + the robot's pose and motion. + rt_set_last_time (int): Used to keep track of the time since the last RT + set was received. It is used to determine when to send a new RT set. + rt_time_min (float): Used to set a minimum time interval between RT sets. + It serves as a threshold for determining when a new RT set should be + initiated based on the agent's movement. + last_update_with_corners (int): Used to store the last time corners were + updated. It's used in conjunction with other attributes to maintain a + consistent update schedule for corners. + timer (QTimer): Used to schedule a call to the `QApplication.instance().quit()` + function every 200 milliseconds, indicating that the worker should + shut down. + compute (lambda): Used to compute the marginal probability of the worker's + variables given the observed data. It takes an index `i` as input and + returns a tuple containing the probability of each variable in the + worker's graph given the observed data, as well as the gradient of the + probability with respect to the variable's value. + update_node_att (QMetaObjectAttribute): Used to update the attributes of + a node in the graph when a new attribute is received from the ROS bag. + update_edge (str): Used to update the edge attributes based on the edge + type. It takes three parameters: `fr`, `to`, and `type`, which are the + index of the current edge, the index of the next edge, and the edge + type, respectively. The method updates the edge attributes based on + the edge type and sets the `current_edge_set` attribute to `True`. + update_edge_att (edge): Used to update the attributes of an edge in the graph. """ def __init__(self, proxy_map, startup_check=False): """ - Initializes member variables and sets up event handling for signals related - to the graph, nodes, edges, and attributes. + Initializes an instance of the SpecificWorker class by setting up various + components such as g2o graph, visualizer, and timers. It also performs + startup checks and connects signals for node and edge updates. Args: - proxy_map (dict): Used to map agent id's to their corresponding DSRGraph - objects. - startup_check (bool): Used to determine whether to run the `startup_check` - method or not when initializing the SpecificWorker object. + proxy_map (dict): Used to store mapping information between the original + graph and the transformed graph for DSR algorithm implementation. + startup_check (Optionalbool): Used to check for any errors in the graph + during initialization. """ super(SpecificWorker, self).__init__(proxy_map) @@ -216,6 +227,8 @@ def __init__(self, proxy_map, startup_check=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) @@ -239,8 +252,9 @@ def setParams(self, params): @QtCore.Slot() def compute(self): """ - Updates the robot's position, orientations, and security polygon based on - the odometry data and the RT graph. + Performs RT odometry computation based on sensor data and updates the + Graph2O graph with landmarks, affordances, and RT edges. It also maintains + counters for valid corners and doors, and rotates the robot if necessary. """ if time.time() - self.elapsed > 1: @@ -290,6 +304,7 @@ def compute(self): # Check if robot pose is inside room polygon + 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): @@ -301,6 +316,8 @@ def compute(self): 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: + 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] @@ -320,23 +337,27 @@ def compute(self): 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 == 4 and len(affordance_nodes) == 0: + if self.rt_set_last_time - self.last_update_with_corners > 3: + print("No affordance nodes active. Rotating robot") + opt_orientation += np.pi/8 + 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) @@ -362,11 +383,10 @@ def initialize_g2o_graph(self): # print("Initializing g2o graph") # get robot pose in room """ - Initializes a Graph-based Object Recognition (G2O) graph for a specific - room by: - 1/ Finding the corners of the room using ROS topics. - 2/ Adding nominal corners to the G2O graph. - 3/ Adding fixed poses to the G2O graph for the robot and doors in the room. + Initializes a Graph2O graph for a specific robot and room by: + * Extracting landmarks from the robot's point cloud + * Calculating nominal corners based on landmarks and room geometry + * Adding fixed poses to the g2o graph for the robot and doors in the room. Returns: bool: 1 if the initialization of the g2o graph was successful, and 0 @@ -424,7 +444,7 @@ def initialize_g2o_graph(self): # 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 = 300 + d = 500 self.security_polygon = QPolygonF() if self.room_polygon is not None: landmark_information = np.array([[0.05, 0.0], @@ -464,20 +484,26 @@ def initialize_g2o_graph(self): 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 - 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 + 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, - door_measured_rt, + None, landmark_information, pose_id=0) - else: - print("Door is not valid") + + 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 @@ -571,18 +597,17 @@ def initialize_g2o_graph(self): def get_displacement(self, odometry): """ - Calculates the displacement of an object based on its odometry data, - computing the lateral displacement, forward displacement, and angular - displacement using a moving window approach. + Computes and updates the displacement values (lateral, forward, and angular) + of a robot based on its odometry data, using a moving average of recent + positions to smooth out the motion. Args: - odometry (3element): Used to store the robot's motion information at - each time step, including its linear displacement, lateral - displacement, and angular displacement. + odometry (3element): An instance of the `Odometry` class, representing + the robot's position and velocity over time. Returns: - 3element: Displacement in three directions (lateral, advance and - angular) calculated using the odometry data from a queue. + 3element: The lateral displacement (in meters), the forward displacement + (in meters) and angular displacement (in radians). """ desplazamiento_avance = 0 @@ -610,13 +635,10 @@ def get_covariance_matrix(self, vertex): constructing the covariance matrix. Args: - vertex (g2overtex): Used to compute the covariance matrix of the - vertices in the graph. + vertex (g2oVertex): Used to represent a vertex in the graph. Returns: - tuple: 2-element, where the first element is a boolean value indicating - whether the covariance matrix was computed or not and the second element - is the actual covariance matrix. + 2tuple: A pair of a boolean result and a covariance matrix. """ cov_vertices = [(vertex.hessian_index(), vertex.hessian_index())] @@ -633,12 +655,13 @@ def get_covariance_matrix(self, vertex): def visualize_g2o_realtime(self, optimizer): """ - Within the `SpecificWorker` class loads an G2O file, visualizes its vertices - and edges in 3D, and updates the visualization in real-time as the optimizer - processes new measurements. + Within the `SpecificWorker` class loads a G2O file, visualizes the vertices + and edges of the graph in a 3D scatter plot, and updates the plot in + real-time as new measurements are received. Args: - optimizer (instance): Used to load G2O files. + optimizer (instance): An object that loads G2O files and provides + access to their vertices and edges for visualization. """ plt.ion() @@ -687,13 +710,14 @@ def update_node_att(self, id: int, attribute_names: [str]): # print("INIT GRAPH") """ - Updates the attributes of a node in a graph, specifically the odometry - node, by appending values to an odometry queue. + Updates an attribute of a node in a graph, specifically the odometry node, + by appending its current position and velocity to a queue for processing. Args: - id (int): Used to represent the node ID of interest for updating attributes. - attribute_names ([str]): An array of strings representing the names - of attributes to be updated on the node. + id (int): Passed as an argument to the function, representing the + unique identifier for the node being updated. + attribute_names ([str]): A list of names of attributes to update on + the node associated with the given ID. """ if id == self.odometry_node_id: @@ -714,21 +738,22 @@ def update_node(self, id: int, type: str): # self.room_initialized = True if self.initialize_g2o_graph() else False # self.init_graph = True """ - Updates an unknown node with the specified ID, performing different actions - based on the type of node it represents. + Updates a node's type, but only if the type is "corner". If it is, the + function also checks if the "room" node has been initialized and sets the + `init_graph` attribute to `True`. Args: - id (int): Used to identify the node being updated. - type (str): Used to specify the node's type, with possible values being - "corner". + id (int): Used to represent the unique identifier of a node. + type (str): Used to indicate the type of node being updated, specifically + whether it is a corner node or not. """ pass def delete_node(self, id: int): """ - Deletes a node from the graph represented by the `SpecificWorker` class, - setting the `room_initialized` attribute to `False`. + Deletes a node from a list maintained by a `SpecificWorker` instance, + setting a flag to indicate that the list has been modified. Args: id (int): Used to identify the node to be deleted. @@ -744,18 +769,17 @@ def delete_node(self, id: int): def update_edge(self, fr: int, to: int, type: str): """ - Updates the room ID and sets the current edge set to true when the type - is "current" or "RT" and the source node is a room and the target node is - "Shadow". It also sets the translation and rotation to set variables when - RT is set for the first time. + Updates the current room ID and RT translation and rotation when the agent + moves from one room to another, and sets the `current_edge_set` variable + to `True`. Args: - fr (int): Used to represent the ID of the current node being processed - in the graph. - to (int): The id of the next node in the graph that the edge is pointing - to. - type (str): Used to determine which room the edge updates are for, - either the current room or RT (room to Shadow) + fr (int): A reference to a node in the graph represented by the class + instance. + to (int): Used as an index to access the target node of an edge in the + graph. + type (str): Used to identify the edge type, which can be either "current" + or "RT". """ if type == "current" and self.g.get_node(fr).type == "room": @@ -787,13 +811,13 @@ def update_edge_att(self, fr: int, to: int, type: str, attribute_names: [str]): def delete_edge(self, fr: int, to: int, type: str): """ - Deletes an edge from a graph with specified source and target nodes, based - on the type of edge. + Deletes an edge from a graph based on its FID (fr), TID (to), and edge + type (type). Args: - fr (int): Given a value of 13. - to (int): Specified as an index of an edge to be deleted. - type (str): Used to specify the edge type to be deleted. + fr (int): Used as the index of the edge to be deleted. + to (int): Used to represent the destination node ID for edge deletion. + type (str): Used to specify the edge type that should be deleted. """ pass diff --git a/agents/long_term_spatial_memory_agent/src/specificworker.py b/agents/long_term_spatial_memory_agent/src/specificworker.py index 813c7f3b..bbe44087 100644 --- a/agents/long_term_spatial_memory_agent/src/specificworker.py +++ b/agents/long_term_spatial_memory_agent/src/specificworker.py @@ -48,85 +48,91 @@ class SpecificWorker(GenericWorker): """ - Performs various tasks related to a specific worker in a multi-agent system, - such as checking for room occupation, updating node attributes, and generating - room images. It also handles insertion and deletion of edges and nodes, and - updates the state of the agent based on certain conditions. + Manages a graph representing an agent's environment and performs various tasks + related to navigation, such as updating node attributes, inserting edges, and + deleting nodes or edges. It also handles startup checks and provides methods + for checking the current room, element level, and room number, as well as + generating a picture of the room and inserting a current edge. Attributes: - Period (int): 200 milliseconds, which is the time interval between updates - of the worker's state. - agent_id (integerint): Used as a unique identifier for each agent in the - swarm. - g (Graph): Used to store the graph representation of the environment that - the worker is working on. It contains nodes, edges, and other attributes - that are relevant for the worker's task. - update_node (UpdateNode): Used to update the state of a node in the graph - based on its ID, type, and affordance state. - update_edge (str): Used to set a new current edge in the graph when there - is no existing current edge and the room node exists. - startup_check (QTimersingleShot): Used to start the application's shutdown - process after a certain period of time (200 milliseconds) since the - worker was created. - rt_api (str): Used for handling robot actions, specifically RT (Remote - Transport) actions. It provides a way to interact with the robot's - internal API to perform actions such as moving or manipulating objects. - inner_api (API): Used to access the internal API of the agent, allowing - the worker to interact with the agent's internal state and perform - actions such as generating room pictures or updating node attributes. - robot_name (str): Used to store the name of the robot agent in the environment. - robot_id (int): Used to store the ID of the robot node in the graph. It - is used to identify the current room and for RT edges. - last_robot_pose (Pose): Used to store the last known pose of the robot in - the environment, which is used for motion planning and collision detection. - robot_exit_pose (Pose): Used to store the pose of the robot when it exits - a room. - state (str): Used to store the current state of the worker, which can be - either "idle", "running", or "crossed". - affordance_node_active_id (int): Used to store the ID of the node representing - the affordance that the robot is currently interacting with. - exit_door_id (int): 0 by default, indicating the door through which the - robot exits a room when it moves to another room. - room_exit_door_id (int): 31 by default, representing the door ID of the - room that the worker is exiting. + Period (str): 20 seconds long, indicating how often the worker will execute + its function. + agent_id (int): Used to store the ID of the agent that this worker represents. + g (Graph): Used to represent the robot's internal graph, which stores + information about the + robot's environment, such as nodes (rooms), edges, and their attributes. + The graph is + used to model the robot's movement and interactions with its environment. + update_node (int): Used to update the state of a specific node in the graph + based on its type (either "active" or "crossed"). It takes an argument + `type`, which can be either "active" or "crossed", and updates the + corresponding attribute of the node with the given ID. + update_edge (Edge): Used to set a new current edge between two nodes when + there is no existing current edge and the room node exists. + startup_check (QTimersingleShot): Set to call the `QApplication.instance().quit` + function after 200 milliseconds of starting up the worker, indicating + that the worker has finished its initialization and is ready to process + tasks. + rt_api (str): Used to store the API for the robot's RT (Real-Time) + communication. It is used to send commands and receive updates from + the robot in real-time. + inner_api (internal): Used to access the agent's inner API for retrieving + information such as room coordinates, door connections, and robot position. + robot_name (str): Used to store the name of the robot. It is set during + the construction of the worker object and can be accessed later for + various purposes such as communication with other agents or saving in + a graph. + robot_id (int): Used to represent the ID of the robot that is currently + in the room. + last_robot_pose (Pose): Updated each time the worker moves, storing the + last known pose of the robot before any movement occurred. + robot_exit_pose (Pose): Used to represent the pose of a robot when it exits + a room. It is used in the `insert_current_edge` method to determine + when a new edge should be inserted as current. + state (str): Used to store the current state of the agent, which can be + either "idle", "crossed", or "completed". + affordance_node_active_id (int): Used to store the ID of the affordance + node that is currently active or being processed by the worker. + exit_door_id (int): 12 by default, indicating the door ID of the room that + the worker exits from when it finishes its task. + room_exit_door_id (int): 4 by default, representing the ID of the door + that marks the exit of a room in the environment. enter_room_node_id (int): Used to store the ID of the room node that the - worker has entered. It is used to check if the worker has reached the - goal state by comparing it with the ID of the goal room node. - graph (Graph): Used to represent the graph of nodes and edges in the - environment, which is updated based on the worker's actions and observations. + worker is currently inside. + graph (Graph): Used to store the robot's internal graph, which represents + the state of the environment and the robot's actions. vertex_size (int): 16 by default, indicating the size of each vertex in - the graph. - not_required_attrs (list): Used to store a list of node attributes that - are not required for the worker's functionality. It helps in avoiding - unnecessary computation and improving performance by skipping the - retrieval of these attributes during node traversal. - long_term_graph (Graph): Used to store the long-term graph of the environment, - which represents the entire state of the environment over time. - global_map (npndarray): Used to store a map of the environment, which is - used for navigation and other purposes. - insert_current_edge (Edge): Used to insert a new edge into the graph with - the current room as the source node and the robot node as the destination - node, indicating that the robot has entered the current room. - timer (QTimer): Used to schedule a call to the `startup_check()` function - after 200 milliseconds of starting the application, which signals the - agent to start its work. - compute (instance): Used to compute the cost of the robot moving from its - current state to a new state. It takes into account factors such as - distance, obstacles, and door states. + pixels. It can be used to adjust the visual representation of the graph + in the worker's internal API. + not_required_attrs (list): Used to specify a list of attributes that are + not required for the worker's functionality. These are attributes that + can be omitted without affecting the worker's behavior or performance. + long_term_graph (Graph): Used to store the internal graph of the agent + over time, allowing the worker to keep track of the agent's long-term + memory and reasoning. + global_map (dict): Used to store the global map image that the worker uses + for navigation. It is initialized with a blank dictionary when the + worker starts and can be modified by calling methods such as + `insert_current_edge` and `update_node`. + insert_current_edge (Edge): Used to insert a new edge in the graph with + the current room as the origin and the destination room, signaling + that the robot has entered the room. + timer (QTimer): Used to schedule a callback function to be called after a + certain time (in milliseconds) has passed. + compute (str): Used to hold the worker's computation result """ def __init__(self, proxy_map, startup_check=False): """ - Initializes an instance of the SpecificWorker class by setting up various - graph structures, defining variables, and connecting signals for updating - node and edge positions. + Initializes an object of the `SpecificWorker` class, setting properties + and connecting signals for updating node and edge positions in a graph + representing a spatial memory environment. Args: - proxy_map (igraphGraph): Used to specify the graph that represents the - environment for the worker agent. - startup_check (bool): Used to check if the agent has already started. - If set to `False`, it will call the `startup_check` method instead, - which initializes the internal state of the agent. + proxy_map (igraphGraph): Passed to the superclass's constructor. It + represents the graph that the SpecificWorker class will work on. + startup_check (bool): Used to determine whether to run a check during + startup, which involves checking for validity of graph edges. """ super(SpecificWorker, self).__init__(proxy_map) @@ -196,14 +202,11 @@ def __del__(self): def setParams(self, params): """ - Sets parameters and modifies the state of a Room object by removing a - self-edge, storing an exit door ID, setting attribute values for doors, - and reading an entrance door node to add an other_side_door attribute with - the name of the exit door in the new room. + Sets the parameters for a worker instance, specifically removing a self-edge + from a room and updating various attributes related to doors. Args: - params (object): Passed in from outside the function to be processed - or modified. + params (object): Used to set various attributes of the room and its doors. Returns: Boolean: True. @@ -242,8 +245,8 @@ def compute(self): # print(origin_level, destination_level) """ - Determines the state of a worker based on the current state of the graph - and performs appropriate actions according to the state. + Determines the current state of a worker and calls the appropriate method + to update its state based on the current state machine. """ match self.state: @@ -268,8 +271,9 @@ def compute(self): 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 """ - Generates high-quality summaries of Java code given to it, checks if any - "current" edge exists, and if not, associates doors based on their proximity. + Determines the robot's affordance in the current room and calculates the + pose of the door to reach the center of a new room. It also associates + doors, updates node attributes, and changes the state of the worker. """ aff_cross_nodes = [node for node in self.g.get_nodes_by_type("affordance") if node.attrs["active"].value == True] @@ -311,11 +315,12 @@ def idle(self): 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]] - self.robot_exit_pose = robot_pose + # 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]) @@ -339,6 +344,9 @@ def idle(self): 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 @@ -369,10 +377,8 @@ def idle(self): def crossed(self): # Get parent node of affordance node """ - Manages the node and edge properties for a worker class called `SpecificWorker`, - which is an extension of `GenericWorker`. It retrieves the affordance node, - checks if it has a parent, and updates the exit door ID node with the - connected room name. + Determines the exit door ID for a specific room based on an active affordance + node and updates the state of the worker accordingly. """ affordance_node = self.g.get_node(self.affordance_node_active_id) @@ -397,9 +403,8 @@ def initializing_room(self): # Get room nodes """ - Determines if there are any rooms other than the current room that the - worker is in, and if so, sets the enter room node ID and inserts an edge - to it. It then sets the state to "initializing doors" and prints a message. + Determines and saves the ID of the first non-exiting room node in the + graph, and sets the 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] @@ -416,12 +421,9 @@ def initializing_room(self): def known_room(self): # Get other side door name attribute """ - In the `SpecificWorker` class is responsible for handling the scenario - where the robot needs to move to another room through a door, and there - is no direct path from the current room to the desired room. It traverses - the graph to find the shortest path to the desired room, updates the node's - parent attribute, and inserts a new edge in the graph to connect the robot - to the desired room. + Given to an instance of the SpecificWorker class, identifies the room that + the robot is currently located in and returns relevant information about + it. """ other_side_door_node = self.g.get_node(self.exit_door_id) @@ -519,9 +521,9 @@ def known_room(self): def initializing_doors(self): # Check if node called "room_entry" of type room exists """ - Identifies the doors connected to the current room and updates their - attributes with the names of the rooms they lead to, also associating them - through a pair of tuples. + Identifies and updates the connections between doors in a graph, based on + their types and attributes. It associates doors with each other and updates + node attributes to reflect their relationships. """ exit_edges = [edge for edge in self.g.get_edges_by_type("exit") if edge.destination == self.exit_door_id] @@ -564,15 +566,15 @@ def initializing_doors(self): 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 their nodes and - updating the "other side door name" and "connected room name" attributes - of each door node. + Adds an edge between two nodes in a graph, linking them as doors to each + other's rooms. It also assigns the names of the other door and connected + room to each node. Args: - door_1 (igraphNode): A reference to one of the doors in the graph, - which is used to establish a connection between two rooms. - door_2 (str): 2-dimensional list containing the name of the door and - its room information. + door_1 (str): 2-element list representing a door node name and its + associated room name in an igraph graph. + door_2 (IgraphVertex): Represented as a list containing the room name + and the adjacent room name. """ try: @@ -594,8 +596,7 @@ def associate_doors(self, door_1, door_2): def store_graph(self): """ - Stores the graph data in a file called "graph.pkl" when a room node is - found in the igraph data structure. + Stores the graph data in a pickle file "graph.pkl". """ actual_room_node = self.g.get_node(self.room_exit_door_id) @@ -614,8 +615,8 @@ def store_graph(self): def removing(self): # # Get last number in the name of the room """ - Removes edges and nodes from the graph based on certain room numbers, - updates the long-term graph, and changes the state to "idle". + Removes nodes from the graph that have no incoming or outgoing edges, and + shadows nodes with specific room numbers. """ room_number = self.g.get_node(self.room_exit_door_id).attrs["room_id"].value @@ -645,13 +646,13 @@ def removing(self): def traverse_graph(self, node_id): # Mark the current node as visited and print it """ - Within the SpecificWorker class recursively traverses the graph by following - RT edges from the starting node until reaching the robot's ID or a maximum - depth limit is reached. It inserts vertices and edges into an igraph handle - for each iteration. + Traverses the graph represented by the `Igraph` object `g`. It starts at + a specified node (`node_id`) and explores its RT children, then recursively + traverses the graph to the destination of each RT child. Args: - node_id (int): The unique identifier of the node to be traversed. + node_id (int): Represented by the `g.get_node()` method, which retrieves + a node from the graph based on its ID. """ node = self.g.get_node(node_id) @@ -664,13 +665,11 @@ def traverse_graph(self, node_id): def traverse_igraph(self, node): """ - Navigates through the graph by starting at a given vertex, identifying its - successors, and inserting vertices and edges to preserve the DSR property - of the graph. It then recursively traverses the successor vertices until - no more updates are needed. + Traverses the graph and inserts DSR vertices and edges based on certain conditions. Args: - node (igraphVertex): Representing an existing vertex in the graph. + node (igraphVertex): An instance of vertex in a graph representing a + node with its attributes such as index, name, room ID, and level. """ vertex_successors = self.graph.successors(node.index) @@ -687,11 +686,12 @@ def traverse_igraph(self, node): def insert_igraph_vertex(self, node): """ - Adds a new vertex to an igraph graph, handling various attributes and edges - based on their types and values. + Adds a new vertex to an igraph graph based on input node data and updates + the graph's vertices with the corresponding attributes. Args: - node (igraphNode): Passed to insert a new vertex into an existing graph. + node (igraphNode): Used to represent a vertex to be added to an igraph + graph. It contains information such as the vertex name, ID, and type. """ self.graph.add_vertex(name=node.name, id=node.id, type=node.type) @@ -731,14 +731,13 @@ def insert_igraph_vertex(self, node): def insert_dsr_vertex(self, parent_name, node): # print("Inserting vertex", node["name"], node["type"]) """ - Inserts a new node into a graph, updating the node's attributes and parent - relationship. + Inserts a new node into a graph, setting its parent node and attributes + based on the provided node object. Args: - parent_name (str): Required, which refers to the name of a vertex in - the graph that will serve as the parent for the new node being inserted. - node (Node): Passed as an instance of the Node class representing a - vertex to be inserted into the graph. + parent_name (str): Used to specify the name of the parent node that + will hold the newly created node as a child. + node (Python): Passed as an instance of class `Node`. """ new_node = Node(agent_id=self.agent_id, type=node["type"], name=node["name"]) @@ -757,12 +756,14 @@ def insert_igraph_edge(self, edge): # Search for the origin and destination nodes in the graph """ - Adds an edge to an igraph object, specifying the origin and destination - nodes, as well as the rotation and translation values. + Adds an edge to an igraph object, providing the origin and destination + nodes and their respective attributes (translation and rotation) for the + edge. Args: - edge (igraphEdge): Passed as a whole object, containing attributes - such as origin, destination, rt translation, and rotation euler xyz. + edge (igraphEdge): An object containing information about an edge to + be added to an igraph graph, including its origin and destination + nodes, as well as rotation and translation attributes. """ origin_node = self.graph.vs.find(id=edge.origin) @@ -778,15 +779,15 @@ def insert_dsr_edge(self, org, dest): # print("ORG::", org) # self.insert_dsr_vertex(dest) """ - Modifies an existing edge in a graph by adding new attributes representing - RT translation and rotation, and then inserts or assigns the updated edge - to the graph. + Modifies an existing edge in a graph by adding new attributes "rt_translation" + and "rt_rotation_euler_xyz". It then inserts or assigns the modified edge + into the graph. Args: - org (Agent): Either None or an existing node object in the graph, - representing the source node of the edge to be inserted. - dest (Agent): Used to specify the target node in the graph for the new - edge. + org (Agent): Represented as a Node object in the graph, containing + information such as id, name, and rotation. + dest (Agent): Used to represent the target node or edge of the graph, + which will be updated with the new RT value and rotation information. """ if org is None: @@ -819,8 +820,7 @@ def insert_dsr_edge(self, org, dest): def draw_graph(self): """ Clears the axis, layouts the graph using the Kamada-Kawai algorithm, and - then plots the vertices and edges with labels and arrows. It also displays - node names. + plots arrows for edges. It also adds node labels and sets xy limits. """ self.ax.clear() @@ -849,16 +849,16 @@ def draw_graph(self): def check_element_room_number(self, node_id): """ - Determines the room number associated with a given node ID and returns it - if found, or -1 otherwise. + Retrieves the room ID associated with a given node ID through attribute + retrieval and returns it if successful, or -1 otherwise. Args: - node_id (str): Passed as an argument to the function, representing the - ID of a node in the graph. + node_id (str): Used to identify the node whose room number needs to + be retrieved. Returns: - int: The room ID of a given node or -1 if an error occurs when trying - to retrieve the room ID. + int: The room ID of a given node's element or -1 if an error occurs + when trying to retrieve the room ID. """ node = self.g.get_node(node_id) @@ -871,15 +871,16 @@ def check_element_room_number(self, node_id): def check_element_level(self, node_id): """ - Within the SpecificWorker class determines an element's level based on an - attribute value and returns that value if found, or -1 if not. + Within SpecificWorker, a subclass of GenericWorker, retrieves the element + level attribute from a given node and returns its value if found, or -1 + otherwise. It also checks the robot position and adjusts door connections + in the agent's internal graph if necessary. Args: - node_id (str): Used to identify the node whose element level will be - checked. + node_id (str): Used as an identifier for a specific node in the graph. Returns: - int: Element level of a node. + integerints: The element level of the node with the given `node_id`. """ node = self.g.get_node(node_id) @@ -927,12 +928,13 @@ def check_element_level(self, node_id): def generate_room_picture(self, room_node_id): """ - Generates an image of a room based on its node ID, retrieves and prints - the attributes of the room node, and draws the room polygon and doors. + Within the `SpecificWorker` class retrieves and processes data related to + a specific room, including its ID, edges with a certain type, and translation + attribute. It also draws the room polygon and doors. Args: - room_node_id (str): Used to retrieve a Node object representing the - room node for which a picture should be generated. + room_node_id (str): Used to identify the node representing the room + for which a picture needs to be generated. """ room_node = self.g.get_node(room_node_id) @@ -971,11 +973,12 @@ def generate_room_picture(self, room_node_id): def insert_current_edge(self, room_id): # Insert current edge to the room """ - Inserts or assigns an edge into the graph, with the `room_id` as the tail - id and the `self.agent_id` as the head id. + Inserts or assigns an edge in the graph with the given `room_id` as its + tail and the current agent's ID as its head. Args: - room_id (str): A unique identifier for a particular room. + room_id (str): Passed as an argument to the function, representing the + ID of the current room that the agent is located in. """ current_edge = Edge(room_id, room_id, "current", self.agent_id) @@ -992,14 +995,15 @@ def update_node_att(self, id: int, attribute_names: [str]): def update_node(self, id: int, type: str): """ - Updates an affordance node's state based on its active ID, printing various - attributes and changing the node's state to "crossed" if it is completed - and not active. + Updates the state of an affordance node based on its ID and type. If the + active affordance node's state is "completed" and its active status is + false, it transitions to the "crossed" state. Args: - id (int): Used to identify the node for which the affordance state - should be updated. - type (str): Used to identify the node's type. + id (int): Passed as an argument representing the unique identifier of + the node being updated. + type (str): Passed as an argument to the function, indicating the type + of node being updated. """ if id == self.affordance_node_active_id: @@ -1021,16 +1025,13 @@ def update_edge(self, fr: int, to: int, type: str): # TODO: Posible problema si tocas el nodo room en la interfaz gráfica """ - Updates an edge in the graph based on the type and values of its arguments. - If there is no current edge with the specified type, it inserts a new edge - and sets it as the current one. + Determines if there is a current edge connecting two nodes in a graph based + on specific conditions and updates the current edge if necessary. Args: - fr (int): Used to represent the ID of the current room door node that - the function operates on. - to (int): The ID of the target node to which the edge is to be updated. - type (str): Used to specify the type of edge to be updated, which can - be either "RT" or "current". + fr (int): A reference to a specific node in the graph. + to (int): Referred to as the ID of a node in the graph. + type (str): Set to "RT". It represents the type of edge being updated. """ 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: From d2d97ef29a231650116ab83518c50e609ff31c14 Mon Sep 17 00:00:00 2001 From: "komment-ai[bot]" <122626893+komment-ai[bot]@users.noreply.github.com> Date: Thu, 11 Jul 2024 09:16:04 +0000 Subject: [PATCH 05/10] Added comments to 28 functions across 2 files --- .komment/00000.json | 408 +++++++++--------- .komment/komment.json | 5 +- .../src/long_term_graph.py | 83 ++-- .../src/specificworker.py | 386 +++++++++-------- 4 files changed, 480 insertions(+), 402 deletions(-) diff --git a/.komment/00000.json b/.komment/00000.json index 14b83b66..109ae982 100644 --- a/.komment/00000.json +++ b/.komment/00000.json @@ -4,42 +4,42 @@ "path": "agents/long_term_spatial_memory_agent/src/long_term_graph.py", "content": { "structured": { - "description": "A 2D environment with obstacles and a player, and uses a probabilistic algorithm to compute the shortest path between two points in the environment while avoiding obstacles. It also provides functions to draw the environment and its objects on a figure using Matplotlib, and to compute the projective coordinates of the corners of a room in the environment frame. Additionally, it defines functions to get the list of objects in a room by type and their projective coordinates in the room frame. The code uses the Geometry package for computing shortest paths and the Matplotlib library for drawing visualizations.", + "description": "A class RoomReconstructionPlotter and uses it to plot a metric map, including rooms and doors, based on a graph representation of a building. It utilizes various functions from the scipy library for geometric calculations and the matplotlib library for plotting. The code first loads a building graph from a text file, then calculates the room corners and their object coordinates in the room frame using recursive functions get_room_corners_recursive and get_room_objects_coordinates. Finally, it draws the rooms and doors on a matplotlib figure using annotate and arrow functions.", "items": [ { - "id": "7453d1a9-2093-7088-9941-e01ea3c40300", + "id": "4b9ea1e4-30c5-c68b-774f-774fe4b224b6", "ancestors": [], - "description": "Computes and draws a graph representation of a long-term memory space, allowing for door-based navigation and metric reconstruction. It takes in a set of rooms and their connections as well as a base room name and computes the metric map between them.", + "description": "Draws a long-term graph representation of a building's layout and room usage, allowing for interactive exploration and analysis of the data. It provides methods to draw the graph, add points, and display room names and door connections.", "attributes": [ { "name": "g", - "type_name": "Graph", - "description": "Used to store a graph representation of a long-term spatial memory task. It contains nodes, edges, and types (rooms, doors, walls) that are used to compute the metric map." + "type_name": "instance", + "description": "Used to specify the color scheme for the graph, where `'r'` represents red, `'b'` represents blue, and `-` represents gray." }, { "name": "read_graph", "type_name": "instance", - "description": "Used to read a graph from a pickled file. It returns a `Subgraph` object representing the graph." + "description": "Used to read a graph from a file specified by the user." }, { "name": "fig", "type_name": "matplotlibfigureFigure", - "description": "Used to store the figure object that will be drawn with the graph." + "description": "Used to store the figure object that represents the graph." }, { "name": "ax", - "type_name": "Axes", - "description": "Used to store the axis object for plotting the graph." + "type_name": "matplotlibaxesAxes", + "description": "Used to interact with a specific axis object in a figure. It provides methods for adding plots, annotations, and other visual elements to the axis." }, { "name": "fig_2", - "type_name": "MatplotlibFigure", - "description": "Used to store a figure object used for drawing the graph." + "type_name": "matplotlibfigureFigure", + "description": "Used to store the figure object for the metric reconstruction plot." }, { "name": "ax_2", - "type_name": "Axis", - "description": "Used to represent the graph's second axis, which is used for the metric reconstruction \nplot. It provides functions to set the title, labels, and limits for this axis." + "type_name": "matplotlibfigureFigure", + "description": "Used to represent the second plot, which shows the metric reconstruction." } ], "name": "LongTermGraph", @@ -51,20 +51,20 @@ "comment": null }, "item_type": "class", - "length": 244, + "length": 269, "docLength": null }, { - "id": "89c82564-d052-e2bf-6549-98b7754116ea", + "id": "77430e1d-d7e6-50b3-8b46-4c2cab75420d", "ancestors": [ - "7453d1a9-2093-7088-9941-e01ea3c40300" + "4b9ea1e4-30c5-c68b-774f-774fe4b224b6" ], - "description": "In the LongTermGraph class reads a graph from a file, creates subplots for metric reconstruction and LTSM display, and initializes the graph object.", + "description": "Reads a graph from a file, creates two subplots for visualization, and sets up the x-axis and y-axis labels.", "params": [ { "name": "file_name", "type_name": "str", - "description": "Used to specify the name of the graph file to read." + "description": "Used to specify the path to a GraphML file from which the graph structure is read." } ], "returns": null, @@ -81,23 +81,23 @@ "docLength": null }, { - "id": "97a1ea39-6c18-2888-1d49-0d9df0c43f81", + "id": "81650769-f298-6a87-d146-f6ccf1580992", "ancestors": [ - "7453d1a9-2093-7088-9941-e01ea3c40300" + "4b9ea1e4-30c5-c68b-774f-774fe4b224b6" ], - "description": "Generates a graph visualization of a subgraph of a larger graph, based on node and edge types. It uses the `layout` parameter to specify the layout algorithm and colors nodes and edges based on their type.", + "description": "Generates a graph representation of a long-term state machine (LTSM) based on its adjacency matrix. It creates the graph layout, defines edge colors, and adds node and edge labels.", "params": [ { "name": "only_rooms", "type_name": "bool", - "description": "Used to filter the nodes and edges displayed on the graph based on their types, with \"room\" being the default value for only rooms are shown." + "description": "Used to filter nodes based on their type. It restricts the subgraph to only include nodes labeled as \"room\"." } ], "returns": null, "name": "draw_graph", "location": { - "start": 220, - "insert": 221, + "start": 253, + "insert": 254, "offset": " ", "indent": 8, "comment": null @@ -1163,137 +1163,137 @@ "path": "agents/long_term_spatial_memory_agent/src/specificworker.py", "content": { "structured": { - "description": "an AI agent that navigates through a 3D space and interacts with objects in that space using a deep reinforcement learning algorithm. The agent maintains a graph representation of its environment and uses this graph to plan its actions and learn from its experiences. The code includes various methods for updating the agent's internal state, such as updating its node attributions, updating its node, and deleting nodes or edges from its graph. Additionally, it includes methods for generating room pictures, inserting current edges, and checking for startup. Overall, this code provides a comprehensive framework for an AI agent to navigate and interact with its environment using deep reinforcement learning.", + "description": "A class called `DSR` that implements a graph-based representation of a robot's environment and its interactions with objects in that environment. The `DSR` class has several methods for updating and manipulating the graph, including inserting new nodes or edges, updating node attributes, deleting nodes or edges, and more. These methods use various high-level packages such as `QApplication`, `QTimer`, `QDebug`, and `pickle` to perform their operations.\n\nOverall, the code defines a system for representing complex environmental interactions between a robot and its surroundings, using graph theory concepts and Python's high-level packages.", "items": [ { - "id": "14d16d1c-f321-80b9-164e-7904534324da", + "id": "8e2d2981-1343-51a0-0a43-35e06cb1288a", "ancestors": [], - "description": "Manages a graph representing an agent's environment and performs various tasks related to navigation, such as updating node attributes, inserting edges, and deleting nodes or edges. It also handles startup checks and provides methods for checking the current room, element level, and room number, as well as generating a picture of the room and inserting a current edge.", + "description": "Manages a robot's interactions with its environment, including moving through doors, creating new rooms, and updating node attributes. It also generates images of rooms and handles long-term graph management.", "attributes": [ { "name": "Period", "type_name": "str", - "description": "20 seconds long, indicating how often the worker will execute its function." + "description": "Used to set a fixed time interval for the worker to perform its function, allowing to schedule the work in a more organized manner." }, { "name": "agent_id", "type_name": "int", - "description": "Used to store the ID of the agent that this worker represents." + "description": "Used to identify the worker as a specific agent, such as a robot or human, within the framework." }, { "name": "g", - "type_name": "Graph", - "description": "Used to represent the robot's internal graph, which stores information about the \nrobot's environment, such as nodes (rooms), edges, and their attributes. The graph is \nused to model the robot's movement and interactions with its environment." + "type_name": "igraph", + "description": "A weighted graph that represents the environment of the agent. It stores the nodes (rooms) and edges between them, which represent the doors connecting the rooms. The graph is used to determine the robot's movement and interactions with its environment." }, { "name": "update_node", - "type_name": "int", - "description": "Used to update the state of a specific node in the graph based on its type (either \"active\" or \"crossed\"). It takes an argument `type`, which can be either \"active\" or \"crossed\", and updates the corresponding attribute of the node with the given ID." + "type_name": "str", + "description": "Used to update a specific node in the graph based on its ID. It takes one argument, which is the type of update (either \"door\" or \"room\"), and performs different actions depending on the update type." }, { "name": "update_edge", - "type_name": "Edge", - "description": "Used to set a new current edge between two nodes when there is no existing current edge and the room node exists." + "type_name": "str", + "description": "Used to update the edge type of a specific edge in the graph. It takes two arguments: the first is the id of the starting node, and the second is the id of the ending node, as well as an optional third argument which is the new edge type to be set for that edge." }, { "name": "startup_check", "type_name": "QTimersingleShot", - "description": "Set to call the `QApplication.instance().quit` function after 200 milliseconds of starting up the worker, indicating that the worker has finished its initialization and is ready to process tasks." + "description": "Used to check for possible termination of the worker after a specific timeout period of 200 milliseconds." }, { "name": "rt_api", - "type_name": "str", - "description": "Used to store the API for the robot's RT (Real-Time) communication. It is used to send commands and receive updates from the robot in real-time." + "type_name": "instance", + "description": "Used for accessing the RT API of the environment to get information about the robot's position, orientation, and other relevant data." }, { "name": "inner_api", "type_name": "internal", - "description": "Used to access the agent's inner API for retrieving information such as room coordinates, door connections, and robot position." + "description": "Used for communication between different agents in a multi-agent environment, allowing them to exchange information and coordinate their actions." }, { "name": "robot_name", "type_name": "str", - "description": "Used to store the name of the robot. It is set during the construction of the worker object and can be accessed later for various purposes such as communication with other agents or saving in a graph." + "description": "Used as the ID of the robot in the internal graph, which is generated based on the agent's name." }, { "name": "robot_id", "type_name": "int", - "description": "Used to represent the ID of the robot that is currently in the room." + "description": "Used to represent the unique identifier of the robot agent in the environment." }, { "name": "last_robot_pose", - "type_name": "Pose", - "description": "Updated each time the worker moves, storing the last known pose of the robot before any movement occurred." + "type_name": "3D", + "description": "Used to store the last known position of the robot, which can be used to determine the robot's motion and interaction with its environment." }, { "name": "robot_exit_pose", - "type_name": "Pose", - "description": "Used to represent the pose of a robot when it exits a room. It is used in the `insert_current_edge` method to determine when a new edge should be inserted as current." + "type_name": "str", + "description": "3D pose representation of the robot when it exits a room. It is used to check if the robot has reached its destination and to generate an image of the room." }, { "name": "state", "type_name": "str", - "description": "Used to store the current state of the agent, which can be either \"idle\", \"crossed\", or \"completed\"." + "description": "Used to store the current state of the worker (either \"idle\", \"busy\", or \"crossed\") indicating whether the worker is available or not for new tasks." }, { "name": "affordance_node_active_id", "type_name": "int", - "description": "Used to store the ID of the affordance node that is currently active or being processed by the worker." + "description": "Used to store the ID of the affordance node that indicates when a robot reaches a certain affordance, it will stop moving and wait for further instructions." }, { "name": "exit_door_id", "type_name": "int", - "description": "12 by default, indicating the door ID of the room that the worker exits from when it finishes its task." + "description": "13 by default, representing the index of the door node that marks the exit of a room in the graph. It is used to identify the door through which the robot exits a room when it needs to navigate to another room." }, { "name": "room_exit_door_id", "type_name": "int", - "description": "4 by default, representing the ID of the door that marks the exit of a room in the environment." + "description": "0-indexed. It represents the ID of the door connecting a room to the environment. It is used to identify the door through which the robot can exit a room." }, { "name": "enter_room_node_id", "type_name": "int", - "description": "Used to store the ID of the room node that the worker is currently inside." + "description": "Used to store the ID of the room node that the worker enters after crossing the door." }, { "name": "graph", - "type_name": "Graph", - "description": "Used to store the robot's internal graph, which represents the state of the environment and the robot's actions." + "type_name": "igraphGraph", + "description": "Used to store the agent's internal graph, which represents the environment and the robot's movements." }, { "name": "vertex_size", "type_name": "int", - "description": "16 by default, indicating the size of each vertex in pixels. It can be used to adjust the visual representation of the graph in the worker's internal API." + "description": "16 by default, which means that each vertex in the graph has a size of 16 pixels in the internal graph representation." }, { "name": "not_required_attrs", "type_name": "list", - "description": "Used to specify a list of attributes that are not required for the worker's functionality. These are attributes that can be omitted without affecting the worker's behavior or performance." + "description": "Used to store a list of attribute names that are not required for the worker's functionality, but may be useful for debugging or other purposes." }, { "name": "long_term_graph", - "type_name": "Graph", - "description": "Used to store the internal graph of the agent over time, allowing the worker to keep track of the agent's long-term memory and reasoning." + "type_name": "igraphGraph", + "description": "Used for storing the long-term graph of the environment, which is different from the short-term graph used for navigation." }, { "name": "global_map", - "type_name": "dict", - "description": "Used to store the global map image that the worker uses for navigation. It is initialized with a blank dictionary when the worker starts and can be modified by calling methods such as `insert_current_edge` and `update_node`." + "type_name": "igraphGraph", + "description": "Used to store the global map of the environment. It is used to represent the entire environment and its connections, rather than just the local area around a single agent or node." }, { "name": "insert_current_edge", "type_name": "Edge", - "description": "Used to insert a new edge in the graph with the current room as the origin and the destination room, signaling that the robot has entered the room." + "description": "Used to insert a new edge in the graph with the current room as the source and the robot as the destination, with the \"current\" edge type." }, { "name": "timer", "type_name": "QTimer", - "description": "Used to schedule a callback function to be called after a certain time (in milliseconds) has passed." + "description": "Used to schedule the worker's update loop to run after a certain delay (200 milliseconds in this case)." }, { "name": "compute", - "type_name": "str", - "description": "Used to hold the worker's computation result" + "type_name": "instance", + "description": "Used to compute the worker's output for a given input." } ], "name": "SpecificWorker", @@ -1305,25 +1305,25 @@ "comment": null }, "item_type": "class", - "length": 537, + "length": 568, "docLength": null }, { - "id": "f5c0bd08-589a-4b8b-a14e-7fd68022b8be", + "id": "241962dc-0914-a79d-9b40-5909c260c9ce", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Initializes an object of the `SpecificWorker` class, setting properties and connecting signals for updating node and edge positions in a graph representing a spatial memory environment.", + "description": "Initializes an object of the `SpecificWorker` class, setting its graph, period, and other properties.", "params": [ { "name": "proxy_map", "type_name": "igraphGraph", - "description": "Passed to the superclass's constructor. It represents the graph that the SpecificWorker class will work on." + "description": "Used to initialize the graph object for the specific worker's long-term spatial memory." }, { "name": "startup_check", "type_name": "bool", - "description": "Used to determine whether to run a check during startup, which involves checking for validity of graph edges." + "description": "Used to check if the graph has been modified since the last startup. If set to True, it will call the `startup_check` method, otherwise it will skip it." } ], "returns": null, @@ -1340,16 +1340,16 @@ "docLength": null }, { - "id": "f9acbd22-c01b-23b7-6f4f-b4fa40fec045", + "id": "6bfee08c-8e0e-7abf-324a-8ea14b6e006b", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Sets the parameters for a worker instance, specifically removing a self-edge from a room and updating various attributes related to doors.", + "description": "Sets parameters for an object, removes a self-edge from a room, stores ID of the exit door, names both doors with the other side door attribute, and reads entrance door node to add other side door attributes.", "params": [ { "name": "params", "type_name": "object", - "description": "Used to set various attributes of the room and its doors." + "description": "Passed to set parameters for an object of class `Room`." } ], "returns": { @@ -1369,11 +1369,11 @@ "docLength": null }, { - "id": "9f58c232-e34f-ca91-4848-7ab916c1e754", + "id": "ed2b32ce-b574-b9a6-4c4b-a7dafa3d6b90", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Determines the current state of a worker and calls the appropriate method to update its state based on the current state machine.", + "description": "Determines the current state of the worker and performs the appropriate action based on that state, including idling, crossing, crossing, initializing rooms, knowing rooms, initializing doors, storing the graph, and removing nodes.", "params": [], "returns": null, "name": "compute", @@ -1389,11 +1389,11 @@ "docLength": null }, { - "id": "8089bfc6-1aec-89b3-ff4c-2221ef94ad24", + "id": "250b555a-b6a6-759f-324b-d064b390a847", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Determines the robot's affordance in the current room and calculates the pose of the door to reach the center of a new room. It also associates doors, updates node attributes, and changes the state of the worker.", + "description": "Performs affordance-based navigation for an autonomous robot, specifically:\n\n1. Identifies active affordance nodes in the short-term graph.\n2. Checks if any \"current\" edge exists and if so, identifies the door node leading to the next room.\n3. Computes the final robot pose and door pose in the new room reference frame using the inner API.", "params": [], "returns": null, "name": "idle", @@ -1405,21 +1405,21 @@ "comment": null }, "item_type": "method", - "length": 75, + "length": 76, "docLength": null }, { - "id": "59952246-8d88-ffad-ea44-e45c226538f7", + "id": "c19737e3-8045-f7b9-134c-d9f08ec98f43", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Determines the exit door ID for a specific room based on an active affordance node and updates the state of the worker accordingly.", + "description": "Determines whether a node represents an exit door and, if so, updates the state of the `SpecificWorker` instance to reflect this information.", "params": [], "returns": null, "name": "crossed", "location": { - "start": 268, - "insert": 270, + "start": 269, + "insert": 271, "offset": " ", "indent": 8, "comment": null @@ -1429,17 +1429,17 @@ "docLength": null }, { - "id": "3d97375c-3e55-c7b9-1d40-4fee711096bc", + "id": "5e3379bd-db81-d1a5-8149-2cb24d34ac7c", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Determines and saves the ID of the first non-exiting room node in the graph, and sets the state to \"initializing doors\".", + "description": "Determines and remembers the room node ID that the worker will enter after leaving the current room, and updates the state to \"initializing doors\".", "params": [], "returns": null, "name": "initializing_room", "location": { - "start": 288, - "insert": 291, + "start": 289, + "insert": 292, "offset": " ", "indent": 8, "comment": null @@ -1449,17 +1449,17 @@ "docLength": null }, { - "id": "81c984ff-ff5e-afab-5d46-5440953dbe7f", + "id": "11cbfdf2-e11b-e0aa-4947-9344b9dd510a", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Given to an instance of the SpecificWorker class, identifies the room that the robot is currently located in and returns relevant information about it.", + "description": "Within the SpecificWorker class updates the RT object's position and rotation based on the last known affordance pose, taking into account the robot's movement and the associated door found in the global map.", "params": [], "returns": null, "name": "known_room", "location": { - "start": 302, - "insert": 304, + "start": 303, + "insert": 305, "offset": " ", "indent": 8, "comment": null @@ -1469,17 +1469,17 @@ "docLength": null }, { - "id": "1a1e3b3c-b20c-c79f-5c42-f0dbaa2e4333", + "id": "67025356-2f92-3c84-ef43-e9f81d799e94", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Identifies and updates the connections between doors in a graph, based on their types and attributes. It associates doors with each other and updates node attributes to reflect their relationships.", + "description": "1) finds edges leading to the exit door, 2) checks if any match edges exist, and 3) updates node attributes and associations for doors involved.", "params": [], "returns": null, "name": "initializing_doors", "location": { - "start": 396, - "insert": 398, + "start": 397, + "insert": 399, "offset": " ", "indent": 8, "comment": null @@ -1489,28 +1489,28 @@ "docLength": null }, { - "id": "517f46f2-3ff6-5e9f-4243-ba8f95693b69", + "id": "cb77cc09-6f32-8aa9-244d-8bd8ff15aafd", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Adds an edge between two nodes in a graph, linking them as doors to each other's rooms. It also assigns the names of the other door and connected room to each node.", + "description": "Adds an edge to a graph between two nodes representing doors, and sets their \"other side door name\" and \"connected room name\" attributes based on the input door names.", "params": [ { "name": "door_1", "type_name": "str", - "description": "2-element list representing a door node name and its associated room name in an igraph graph." + "description": "A name of a door node in the graph." }, { "name": "door_2", - "type_name": "IgraphVertex", - "description": "Represented as a list containing the room name and the adjacent room name." + "type_name": "str", + "description": "Represented as a list of two elements, where each element represents the name of a door node in the graph." } ], "returns": null, "name": "associate_doors", "location": { - "start": 435, - "insert": 437, + "start": 436, + "insert": 438, "offset": " ", "indent": 8, "comment": null @@ -1520,17 +1520,17 @@ "docLength": null }, { - "id": "8b48d263-c9fe-bc96-5749-1000b78b7843", + "id": "8cdc79dc-b155-0387-ae48-8f95fac0413f", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Stores the graph data in a pickle file \"graph.pkl\".", + "description": "Stores the graph data in a file \"graph.pkl\" and retrieves the room node from the graph using its exit door ID, handling any exceptions.", "params": [], "returns": null, "name": "store_graph", "location": { - "start": 454, - "insert": 455, + "start": 455, + "insert": 456, "offset": " ", "indent": 8, "comment": null @@ -1540,17 +1540,17 @@ "docLength": null }, { - "id": "5e9c08ef-5fbc-0d93-b441-f4b177f7bd6a", + "id": "4de8cad4-147a-8d9b-234a-1d5133da4d7d", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Removes nodes from the graph that have no incoming or outgoing edges, and shadows nodes with specific room numbers.", + "description": "Removes nodes from the long-term graph based on their room number, preserving edges that connect rooms with the same number. It first identifies nodes with room numbers matching the input room number, then deletes nodes and their corresponding edges in the long-term graph.", "params": [], "returns": null, "name": "removing", "location": { - "start": 468, - "insert": 470, + "start": 469, + "insert": 471, "offset": " ", "indent": 8, "comment": null @@ -1560,23 +1560,23 @@ "docLength": null }, { - "id": "1e568e41-1cae-638e-c944-d3abc3ff3edd", + "id": "f0040ef8-9762-97a1-634e-f2fbeeea9223", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Traverses the graph represented by the `Igraph` object `g`. It starts at a specified node (`node_id`) and explores its RT children, then recursively traverses the graph to the destination of each RT child.", + "description": "Traverses the graph represented by an igraph object `g` and performs some operations on it. Specifically, it: (1) retrieves a node with the given `node_id`, (2) extracts its RT children edges from the graph, (3) inserts a vertex representing the current node in the graph, and (4) recursively traverses the RT children of each of these nodes.", "params": [ { "name": "node_id", "type_name": "int", - "description": "Represented by the `g.get_node()` method, which retrieves a node from the graph based on its ID." + "description": "A node ID to traverse in the graph." } ], "returns": null, "name": "traverse_graph", "location": { - "start": 494, - "insert": 496, + "start": 495, + "insert": 497, "offset": " ", "indent": 8, "comment": null @@ -1586,23 +1586,23 @@ "docLength": null }, { - "id": "6bb1abc3-e4f5-a8be-8d4f-df44de6d1f92", + "id": "79a1657a-5231-9baa-8144-c7d2b2a58dee", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Traverses the graph and inserts DSR vertices and edges based on certain conditions.", + "description": "In the `SpecificWorker` class recursively traverses the graph, inserting vertices and edges into a DSR graph for each vertex that has a successor with a higher level than itself, and then recursively traversing the successor vertex.", "params": [ { "name": "node", "type_name": "igraphVertex", - "description": "An instance of vertex in a graph representing a node with its attributes such as index, name, room ID, and level." + "description": "Used to represent a vertex in an igraph graph." } ], "returns": null, "name": "traverse_igraph", "location": { - "start": 504, - "insert": 505, + "start": 505, + "insert": 506, "offset": " ", "indent": 8, "comment": null @@ -1612,23 +1612,23 @@ "docLength": null }, { - "id": "6d280869-b88e-6d8b-994f-d1e0dd1cc3de", + "id": "4b40456c-9fcf-758a-2a42-5d328535710a", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Adds a new vertex to an igraph graph based on input node data and updates the graph's vertices with the corresponding attributes.", + "description": "Adds a new vertex to an igraph object, setting its name and ID, as well as assigning values to certain attributes. It also attempts to find an edge connecting the new vertex to a predefined \"other side door\" vertex, based on attribute values.", "params": [ { "name": "node", "type_name": "igraphNode", - "description": "Used to represent a vertex to be added to an igraph graph. It contains information such as the vertex name, ID, and type." + "description": "Passed the vertex to be added to the graph." } ], "returns": null, "name": "insert_igraph_vertex", "location": { - "start": 517, - "insert": 518, + "start": 518, + "insert": 519, "offset": " ", "indent": 8, "comment": null @@ -1638,28 +1638,28 @@ "docLength": null }, { - "id": "8af24c89-c672-9d98-324b-58e820167ab8", + "id": "1ff5dd0f-33d6-0bb1-1947-12b885b5c298", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Inserts a new node into a graph, setting its parent node and attributes based on the provided node object.", + "description": "Inserts a new node into a graph, updating the parent node's attributes and setting the new node's attributes based on the input node.", "params": [ { "name": "parent_name", "type_name": "str", - "description": "Used to specify the name of the parent node that will hold the newly created node as a child." + "description": "Used to specify the name of the parent node to which the new node will be added." }, { "name": "node", - "type_name": "Python", - "description": "Passed as an instance of class `Node`." + "type_name": "dict", + "description": "Passed as an attribute of a vertex." } ], "returns": null, "name": "insert_dsr_vertex", "location": { - "start": 552, - "insert": 554, + "start": 553, + "insert": 555, "offset": " ", "indent": 8, "comment": null @@ -1669,23 +1669,23 @@ "docLength": null }, { - "id": "74eb41e5-e103-40bc-1e4f-dbf64f47284b", + "id": "c43f5780-c5b5-1db6-8f43-382b331bd896", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Adds an edge to an igraph object, providing the origin and destination nodes and their respective attributes (translation and rotation) for the edge.", + "description": "Adds an edge to an igraph object based on the attributes of the given edge, including the translation and rotation of the edge.", "params": [ { "name": "edge", "type_name": "igraphEdge", - "description": "An object containing information about an edge to be added to an igraph graph, including its origin and destination nodes, as well as rotation and translation attributes." + "description": "Passed as an instance of Igraph Edge class, which contains attributes including origin, destination, rt translation, and rotation." } ], "returns": null, "name": "insert_igraph_edge", "location": { - "start": 566, - "insert": 569, + "start": 567, + "insert": 570, "offset": " ", "indent": 8, "comment": null @@ -1695,28 +1695,28 @@ "docLength": null }, { - "id": "067fc9ce-6d2c-50b3-9642-530b595fd5e7", + "id": "43900a48-129f-0ea2-a944-43b63dbaa2ec", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Modifies an existing edge in a graph by adding new attributes \"rt_translation\" and \"rt_rotation_euler_xyz\". It then inserts or assigns the modified edge into the graph.", + "description": "Adds an edge to the graph, connecting a node representing the origin to a node representing the destination, with the given RT values and rotation.", "params": [ { "name": "org", - "type_name": "Agent", - "description": "Represented as a Node object in the graph, containing information such as id, name, and rotation." + "type_name": "instance", + "description": "Representing the start node of the edge to be inserted." }, { "name": "dest", "type_name": "Agent", - "description": "Used to represent the target node or edge of the graph, which will be updated with the new RT value and rotation information." + "description": "Passed as an argument to the function, representing the destination node for which the edge is being inserted." } ], "returns": null, "name": "insert_dsr_edge", "location": { - "start": 578, - "insert": 581, + "start": 579, + "insert": 582, "offset": " ", "indent": 8, "comment": null @@ -1726,17 +1726,17 @@ "docLength": null }, { - "id": "00c1cc7c-1774-8f91-744a-08d2a9dcebe7", + "id": "d6f2e148-3e44-84a9-bf43-dcb827871106", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Clears the axis, layouts the graph using the Kamada-Kawai algorithm, and plots arrows for edges. It also adds node labels and sets xy limits.", + "description": "Draws the graph using Matplotlib, clearing the axis before laying out the vertices and edges. It then plots each edge with an arrow pointing to its tail node, and displays node names at their centers. Finally, it sets limits on the x and y axes.", "params": [], "returns": null, "name": "draw_graph", "location": { - "start": 608, - "insert": 609, + "start": 609, + "insert": 610, "offset": " ", "indent": 8, "comment": null @@ -1746,26 +1746,26 @@ "docLength": null }, { - "id": "0c45547a-d643-a3a4-6841-fb69f9b9d7ac", + "id": "f36cee33-dca3-99ac-1243-f942df6664c6", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Retrieves the room ID associated with a given node ID through attribute retrieval and returns it if successful, or -1 otherwise.", + "description": "Retrieves the room ID associated with a given node ID by accessing the `room_id` attribute of the node's attributes, and returns the room ID if successful, or -1 otherwise.", "params": [ { "name": "node_id", "type_name": "str", - "description": "Used to identify the node whose room number needs to be retrieved." + "description": "Used to retrieve a specific node from the graph." } ], "returns": { "type_name": "int", - "description": "The room ID of a given node's element or -1 if an error occurs when trying to retrieve the room ID." + "description": "The room ID associated with a given node ID." }, "name": "check_element_room_number", "location": { - "start": 633, - "insert": 634, + "start": 634, + "insert": 635, "offset": " ", "indent": 8, "comment": null @@ -1775,26 +1775,26 @@ "docLength": null }, { - "id": "8b8d5f6b-ab49-e8ac-264d-78917524c774", + "id": "03d00d97-d654-dea5-514a-bd8142708d10", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Within SpecificWorker, a subclass of GenericWorker, retrieves the element level attribute from a given node and returns its value if found, or -1 otherwise. It also checks the robot position and adjusts door connections in the agent's internal graph if necessary.", + "description": "Within the SpecificWorker class retrieves an element level attribute from a given node and returns its value. If the attribute is not found, it prints an error message and returns -1.", "params": [ { "name": "node_id", "type_name": "str", - "description": "Used as an identifier for a specific node in the graph." + "description": "Used as an identifier of a node within the graph." } ], "returns": { "type_name": "integerints", - "description": "The element level of the node with the given `node_id`." + "description": "The level of an element with the given node ID." }, "name": "check_element_level", "location": { - "start": 642, - "insert": 643, + "start": 643, + "insert": 644, "offset": " ", "indent": 8, "comment": null @@ -1804,23 +1804,23 @@ "docLength": null }, { - "id": "9cc9cde0-a537-cbb5-f946-cafc99280a81", + "id": "0ea5bdac-20c3-a691-e24f-7e61ffbee8e3", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Within the `SpecificWorker` class retrieves and processes data related to a specific room, including its ID, edges with a certain type, and translation attribute. It also draws the room polygon and doors.", + "description": "Retrieves the attributes of nodes connected by RT edges in a graph, and then prints their origin and destination nodes, as well as any translation attribute found.", "params": [ { "name": "room_node_id", "type_name": "str", - "description": "Used to identify the node representing the room for which a picture needs to be generated." + "description": "Used to retrieve the node ID of the room to be drawn." } ], "returns": null, "name": "generate_room_picture", "location": { - "start": 685, - "insert": 687, + "start": 686, + "insert": 688, "offset": " ", "indent": 8, "comment": null @@ -1830,23 +1830,23 @@ "docLength": null }, { - "id": "a1d95887-ac1d-d1ae-5e4c-e5a373c17054", + "id": "31965a19-6976-7285-2f4b-afb9ec073b71", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Inserts or assigns an edge in the graph with the given `room_id` as its tail and the current agent's ID as its head.", + "description": "Inserts or assigns an edge with specified properties to a graph, specifically the current edge representing the worker's location.", "params": [ { "name": "room_id", "type_name": "str", - "description": "Passed as an argument to the function, representing the ID of the current room that the agent is located in." + "description": "Used to specify the current room id for which the edge is being created or updated." } ], "returns": null, "name": "insert_current_edge", "location": { - "start": 720, - "insert": 722, + "start": 721, + "insert": 723, "offset": " ", "indent": 8, "comment": null @@ -1856,64 +1856,64 @@ "docLength": null }, { - "id": "23efec40-805a-b496-934d-2914ec7c005d", + "id": "f87a4863-6096-8a94-4044-9bfcb8540c8a", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Updates the state of an affordance node based on its ID and type. If the active affordance node's state is \"completed\" and its active status is false, it transitions to the \"crossed\" state.", + "description": "Updates a node's type, checking if it's a door and inserting its corresponding edge in the long-term graph. If the id is the affordance node's active id, it checks if the node is completed and not active, and transitions to the crossed state.", "params": [ { "name": "id", "type_name": "int", - "description": "Passed as an argument representing the unique identifier of the node being updated." + "description": "Used to represent the unique identifier of a node in the graph." }, { "name": "type", "type_name": "str", - "description": "Passed as an argument to the function, indicating the type of node being updated." + "description": "Used to identify the node type, which determines the actions performed on the node when the function is called." } ], "returns": null, "name": "update_node", "location": { - "start": 734, - "insert": 735, + "start": 735, + "insert": 736, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 9, + "length": 39, "docLength": null }, { - "id": "d05c52b1-a4de-66a0-d446-982a037f8152", + "id": "aca8a629-4ef8-e5ba-9748-4191fd060065", "ancestors": [ - "14d16d1c-f321-80b9-164e-7904534324da" + "8e2d2981-1343-51a0-0a43-35e06cb1288a" ], - "description": "Determines if there is a current edge connecting two nodes in a graph based on specific conditions and updates the current edge if necessary.", + "description": "Updates an edge based on its ID, type and node IDs. It checks if the specified edge exists and is not the current edge, and sets it as the current edge if conditions are met.", "params": [ { "name": "fr", "type_name": "int", - "description": "A reference to a specific node in the graph." + "description": "Representative of the ID of the node it originates from." }, { "name": "to", "type_name": "int", - "description": "Referred to as the ID of a node in the graph." + "description": "The ID of the destination node in the graph." }, { "name": "type", "type_name": "str", - "description": "Set to \"RT\". It represents the type of edge being updated." + "description": "Set to \"RT\"." } ], "returns": null, "name": "update_edge", "location": { - "start": 749, - "insert": 753, + "start": 786, + "insert": 790, "offset": " ", "indent": 8, "comment": null diff --git a/.komment/komment.json b/.komment/komment.json index a3dd6f90..17d8fad2 100644 --- a/.komment/komment.json +++ b/.komment/komment.json @@ -1,13 +1,14 @@ { "meta": { "version": "1", - "updated_at": "2024-07-10T09:56:40.559Z", + "updated_at": "2024-07-11T09:15:57.400Z", "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" + "1f636eb0-7f1e-4d1d-b462-6d1eff3533cb", + "d6a72e5c-096b-4179-ac39-3343ea5ec3a5" ] }, "lookup": [ 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 5b497d32..d221b6c4 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 @@ -14,35 +14,34 @@ class LongTermGraph: """ - Computes and draws a graph representation of a long-term memory space, allowing - for door-based navigation and metric reconstruction. It takes in a set of rooms - and their connections as well as a base room name and computes the metric map - between them. + Draws a long-term graph representation of a building's layout and room usage, + allowing for interactive exploration and analysis of the data. It provides + methods to draw the graph, add points, and display room names and door connections. Attributes: - g (Graph): Used to store a graph representation of a long-term spatial - memory task. It contains nodes, edges, and types (rooms, doors, walls) - that are used to compute the metric map. - read_graph (instance): Used to read a graph from a pickled file. It returns - a `Subgraph` object representing the graph. - fig (matplotlibfigureFigure): Used to store the figure object that will - be drawn with the graph. - ax (Axes): Used to store the axis object for plotting the graph. - fig_2 (MatplotlibFigure): Used to store a figure object used for drawing + g (instance): Used to specify the color scheme for the graph, where `'r'` + represents red, `'b'` represents blue, and `-` represents gray. + read_graph (instance): Used to read a graph from a file specified by the + user. + fig (matplotlibfigureFigure): Used to store the figure object that represents the graph. - ax_2 (Axis): Used to represent the graph's second axis, which is used for - the metric reconstruction - plot. It provides functions to set the title, labels, and limits for - this axis. + ax (matplotlibaxesAxes): Used to interact with a specific axis object in + a figure. It provides methods for adding plots, annotations, and other + visual elements to the axis. + fig_2 (matplotlibfigureFigure): Used to store the figure object for the + metric reconstruction plot. + ax_2 (matplotlibfigureFigure): Used to represent the second plot, which + shows the metric reconstruction. """ def __init__(self, file_name): """ - In the LongTermGraph class reads a graph from a file, creates subplots for - metric reconstruction and LTSM display, and initializes the graph object. + Reads a graph from a file, creates two subplots for visualization, and + sets up the x-axis and y-axis labels. Args: - file_name (str): Used to specify the name of the graph file to read. + file_name (str): Used to specify the path to a GraphML file from which + the graph structure is read. """ try: @@ -108,6 +107,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] @@ -125,6 +125,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] @@ -193,6 +194,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 @@ -201,6 +203,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: @@ -222,6 +225,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: @@ -250,14 +282,13 @@ def check_point_in_map(self, rooms_map: dict, point: QPoint): def draw_graph(self, only_rooms=True): """ - Generates a graph visualization of a subgraph of a larger graph, based on - node and edge types. It uses the `layout` parameter to specify the layout - algorithm and colors nodes and edges based on their type. + Generates a graph representation of a long-term state machine (LTSM) based + on its adjacency matrix. It creates the graph layout, defines edge colors, + and adds node and edge labels. Args: - only_rooms (bool): Used to filter the nodes and edges displayed on the - graph based on their types, with "room" being the default value - for only rooms are shown. + only_rooms (bool): Used to filter nodes based on their type. It restricts + the subgraph to only include nodes labeled as "room". """ self.ax_2.clear() diff --git a/agents/long_term_spatial_memory_agent/src/specificworker.py b/agents/long_term_spatial_memory_agent/src/specificworker.py index bbe44087..229f0774 100644 --- a/agents/long_term_spatial_memory_agent/src/specificworker.py +++ b/agents/long_term_spatial_memory_agent/src/specificworker.py @@ -48,91 +48,92 @@ class SpecificWorker(GenericWorker): """ - Manages a graph representing an agent's environment and performs various tasks - related to navigation, such as updating node attributes, inserting edges, and - deleting nodes or edges. It also handles startup checks and provides methods - for checking the current room, element level, and room number, as well as - generating a picture of the room and inserting a current edge. + Manages a robot's interactions with its environment, including moving through + doors, creating new rooms, and updating node attributes. It also generates + images of rooms and handles long-term graph management. Attributes: - Period (str): 20 seconds long, indicating how often the worker will execute - its function. - agent_id (int): Used to store the ID of the agent that this worker represents. - g (Graph): Used to represent the robot's internal graph, which stores - information about the - robot's environment, such as nodes (rooms), edges, and their attributes. - The graph is - used to model the robot's movement and interactions with its environment. - update_node (int): Used to update the state of a specific node in the graph - based on its type (either "active" or "crossed"). It takes an argument - `type`, which can be either "active" or "crossed", and updates the - corresponding attribute of the node with the given ID. - update_edge (Edge): Used to set a new current edge between two nodes when - there is no existing current edge and the room node exists. - startup_check (QTimersingleShot): Set to call the `QApplication.instance().quit` - function after 200 milliseconds of starting up the worker, indicating - that the worker has finished its initialization and is ready to process - tasks. - rt_api (str): Used to store the API for the robot's RT (Real-Time) - communication. It is used to send commands and receive updates from - the robot in real-time. - inner_api (internal): Used to access the agent's inner API for retrieving - information such as room coordinates, door connections, and robot position. - robot_name (str): Used to store the name of the robot. It is set during - the construction of the worker object and can be accessed later for - various purposes such as communication with other agents or saving in - a graph. - robot_id (int): Used to represent the ID of the robot that is currently - in the room. - last_robot_pose (Pose): Updated each time the worker moves, storing the - last known pose of the robot before any movement occurred. - robot_exit_pose (Pose): Used to represent the pose of a robot when it exits - a room. It is used in the `insert_current_edge` method to determine - when a new edge should be inserted as current. - state (str): Used to store the current state of the agent, which can be - either "idle", "crossed", or "completed". + Period (str): Used to set a fixed time interval for the worker to perform + its function, allowing to schedule the work in a more organized manner. + agent_id (int): Used to identify the worker as a specific agent, such as + a robot or human, within the framework. + g (igraph): A weighted graph that represents the environment of the agent. + It stores the nodes (rooms) and edges between them, which represent + the doors connecting the rooms. The graph is used to determine the + robot's movement and interactions with its environment. + update_node (str): Used to update a specific node in the graph based on + its ID. It takes one argument, which is the type of update (either + "door" or "room"), and performs different actions depending on the + update type. + update_edge (str): Used to update the edge type of a specific edge in the + graph. It takes two arguments: the first is the id of the starting + node, and the second is the id of the ending node, as well as an + optional third argument which is the new edge type to be set for that + edge. + startup_check (QTimersingleShot): Used to check for possible termination + of the worker after a specific timeout period of 200 milliseconds. + rt_api (instance): Used for accessing the RT API of the environment to get + information about the robot's position, orientation, and other relevant + data. + inner_api (internal): Used for communication between different agents in + a multi-agent environment, allowing them to exchange information and + coordinate their actions. + robot_name (str): Used as the ID of the robot in the internal graph, which + is generated based on the agent's name. + robot_id (int): Used to represent the unique identifier of the robot agent + in the environment. + last_robot_pose (3D): Used to store the last known position of the robot, + which can be used to determine the robot's motion and interaction with + its environment. + robot_exit_pose (str): 3D pose representation of the robot when it exits + a room. It is used to check if the robot has reached its destination + and to generate an image of the room. + state (str): Used to store the current state of the worker (either "idle", + "busy", or "crossed") indicating whether the worker is available or + not for new tasks. affordance_node_active_id (int): Used to store the ID of the affordance - node that is currently active or being processed by the worker. - exit_door_id (int): 12 by default, indicating the door ID of the room that - the worker exits from when it finishes its task. - room_exit_door_id (int): 4 by default, representing the ID of the door - that marks the exit of a room in the environment. + node that indicates when a robot reaches a certain affordance, it will + stop moving and wait for further instructions. + exit_door_id (int): 13 by default, representing the index of the door node + that marks the exit of a room in the graph. It is used to identify the + door through which the robot exits a room when it needs to navigate + to another room. + room_exit_door_id (int): 0-indexed. It represents the ID of the door + connecting a room to the environment. It is used to identify the door + through which the robot can exit a room. enter_room_node_id (int): Used to store the ID of the room node that the - worker is currently inside. - graph (Graph): Used to store the robot's internal graph, which represents - the state of the environment and the robot's actions. - vertex_size (int): 16 by default, indicating the size of each vertex in - pixels. It can be used to adjust the visual representation of the graph - in the worker's internal API. - not_required_attrs (list): Used to specify a list of attributes that are - not required for the worker's functionality. These are attributes that - can be omitted without affecting the worker's behavior or performance. - long_term_graph (Graph): Used to store the internal graph of the agent - over time, allowing the worker to keep track of the agent's long-term - memory and reasoning. - global_map (dict): Used to store the global map image that the worker uses - for navigation. It is initialized with a blank dictionary when the - worker starts and can be modified by calling methods such as - `insert_current_edge` and `update_node`. + worker enters after crossing the door. + graph (igraphGraph): Used to store the agent's internal graph, which + represents the environment and the robot's movements. + vertex_size (int): 16 by default, which means that each vertex in the graph + has a size of 16 pixels in the internal graph representation. + not_required_attrs (list): Used to store a list of attribute names that + are not required for the worker's functionality, but may be useful for + debugging or other purposes. + long_term_graph (igraphGraph): Used for storing the long-term graph of the + environment, which is different from the short-term graph used for navigation. + global_map (igraphGraph): Used to store the global map of the environment. + It is used to represent the entire environment and its connections, + rather than just the local area around a single agent or node. insert_current_edge (Edge): Used to insert a new edge in the graph with - the current room as the origin and the destination room, signaling - that the robot has entered the room. - timer (QTimer): Used to schedule a callback function to be called after a - certain time (in milliseconds) has passed. - compute (str): Used to hold the worker's computation result + the current room as the source and the robot as the destination, with + the "current" edge type. + timer (QTimer): Used to schedule the worker's update loop to run after a + certain delay (200 milliseconds in this case). + compute (instance): Used to compute the worker's output for a given input. """ def __init__(self, proxy_map, startup_check=False): """ - Initializes an object of the `SpecificWorker` class, setting properties - and connecting signals for updating node and edge positions in a graph - representing a spatial memory environment. + Initializes an object of the `SpecificWorker` class, setting its graph, + period, and other properties. Args: - proxy_map (igraphGraph): Passed to the superclass's constructor. It - represents the graph that the SpecificWorker class will work on. - startup_check (bool): Used to determine whether to run a check during - startup, which involves checking for validity of graph edges. + proxy_map (igraphGraph): Used to initialize the graph object for the + specific worker's long-term spatial memory. + startup_check (bool): Used to check if the graph has been modified + since the last startup. If set to True, it will call the `startup_check` + method, otherwise it will skip it. """ super(SpecificWorker, self).__init__(proxy_map) @@ -202,11 +203,12 @@ def __del__(self): def setParams(self, params): """ - Sets the parameters for a worker instance, specifically removing a self-edge - from a room and updating various attributes related to doors. + Sets parameters for an object, removes a self-edge from a room, stores ID + of the exit door, names both doors with the other side door attribute, and + reads entrance door node to add other side door attributes. Args: - params (object): Used to set various attributes of the room and its doors. + params (object): Passed to set parameters for an object of class `Room`. Returns: Boolean: True. @@ -245,8 +247,10 @@ def compute(self): # print(origin_level, destination_level) """ - Determines the current state of a worker and calls the appropriate method - to update its state based on the current state machine. + Determines the current state of the worker and performs the appropriate + action based on that state, including idling, crossing, crossing, initializing + rooms, knowing rooms, initializing doors, storing the graph, and removing + nodes. """ match self.state: @@ -271,9 +275,12 @@ def compute(self): 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 """ - Determines the robot's affordance in the current room and calculates the - pose of the door to reach the center of a new room. It also associates - doors, updates node attributes, and changes the state of the worker. + Performs affordance-based navigation for an autonomous robot, specifically: + 1/ Identifies active affordance nodes in the short-term graph. + 2/ Checks if any "current" edge exists and if so, identifies the door node + leading to the next room. + 3/ Computes the final robot pose and door pose in the new room reference + frame using the inner API. """ aff_cross_nodes = [node for node in self.g.get_nodes_by_type("affordance") if node.attrs["active"].value == True] @@ -284,7 +291,8 @@ 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) @@ -293,7 +301,7 @@ def idle(self): # 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() + 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) @@ -377,8 +385,8 @@ def idle(self): def crossed(self): # Get parent node of affordance node """ - Determines the exit door ID for a specific room based on an active affordance - node and updates the state of the worker accordingly. + Determines whether a node represents an exit door and, if so, updates the + state of the `SpecificWorker` instance to reflect this information. """ affordance_node = self.g.get_node(self.affordance_node_active_id) @@ -403,8 +411,8 @@ def initializing_room(self): # Get room nodes """ - Determines and saves the ID of the first non-exiting room node in the - graph, and sets the state to "initializing doors". + Determines and remembers the room node ID that the worker will enter after + leaving the current room, and updates the 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] @@ -421,9 +429,9 @@ def initializing_room(self): def known_room(self): # Get other side door name attribute """ - Given to an instance of the SpecificWorker class, identifies the room that - the robot is currently located in and returns relevant information about - it. + Within the SpecificWorker class updates the RT object's position and + rotation based on the last known affordance pose, taking into account the + robot's movement and the associated door found in the global map. """ other_side_door_node = self.g.get_node(self.exit_door_id) @@ -521,9 +529,8 @@ def known_room(self): def initializing_doors(self): # Check if node called "room_entry" of type room exists """ - Identifies and updates the connections between doors in a graph, based on - their types and attributes. It associates doors with each other and updates - node attributes to reflect their relationships. + 1) finds edges leading to the exit door, 2) checks if any match edges + exist, and 3) updates node attributes and associations for doors involved. """ exit_edges = [edge for edge in self.g.get_edges_by_type("exit") if edge.destination == self.exit_door_id] @@ -566,15 +573,14 @@ def initializing_doors(self): def associate_doors(self, door_1, door_2): # Find each door in igraph and update attributes """ - Adds an edge between two nodes in a graph, linking them as doors to each - other's rooms. It also assigns the names of the other door and connected - room to each node. + Adds an edge to a graph between two nodes representing doors, and sets + their "other side door name" and "connected room name" attributes based + on the input door names. Args: - door_1 (str): 2-element list representing a door node name and its - associated room name in an igraph graph. - door_2 (IgraphVertex): Represented as a list containing the room name - and the adjacent room name. + door_1 (str): A name of a door node in the graph. + door_2 (str): Represented as a list of two elements, where each element + represents the name of a door node in the graph. """ try: @@ -596,7 +602,8 @@ def associate_doors(self, door_1, door_2): def store_graph(self): """ - Stores the graph data in a pickle file "graph.pkl". + Stores the graph data in a file "graph.pkl" and retrieves the room node + from the graph using its exit door ID, handling any exceptions. """ actual_room_node = self.g.get_node(self.room_exit_door_id) @@ -615,8 +622,10 @@ def store_graph(self): def removing(self): # # Get last number in the name of the room """ - Removes nodes from the graph that have no incoming or outgoing edges, and - shadows nodes with specific room numbers. + Removes nodes from the long-term graph based on their room number, preserving + edges that connect rooms with the same number. It first identifies nodes + with room numbers matching the input room number, then deletes nodes and + their corresponding edges in the long-term graph. """ room_number = self.g.get_node(self.room_exit_door_id).attrs["room_id"].value @@ -640,19 +649,20 @@ 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 """ - Traverses the graph represented by the `Igraph` object `g`. It starts at - a specified node (`node_id`) and explores its RT children, then recursively - traverses the graph to the destination of each RT child. + Traverses the graph represented by an igraph object `g` and performs some + operations on it. Specifically, it: (1) retrieves a node with the given + `node_id`, (2) extracts its RT children edges from the graph, (3) inserts + a vertex representing the current node in the graph, and (4) recursively + traverses the RT children of each of these nodes. Args: - node_id (int): Represented by the `g.get_node()` method, which retrieves - a node from the graph based on its ID. + node_id (int): A node ID to traverse in the graph. """ node = self.g.get_node(node_id) @@ -665,11 +675,13 @@ def traverse_graph(self, node_id): def traverse_igraph(self, node): """ - Traverses the graph and inserts DSR vertices and edges based on certain conditions. + In the `SpecificWorker` class recursively traverses the graph, inserting + vertices and edges into a DSR graph for each vertex that has a successor + with a higher level than itself, and then recursively traversing the + successor vertex. Args: - node (igraphVertex): An instance of vertex in a graph representing a - node with its attributes such as index, name, room ID, and level. + node (igraphVertex): Used to represent a vertex in an igraph graph. """ vertex_successors = self.graph.successors(node.index) @@ -686,12 +698,13 @@ def traverse_igraph(self, node): def insert_igraph_vertex(self, node): """ - Adds a new vertex to an igraph graph based on input node data and updates - the graph's vertices with the corresponding attributes. + Adds a new vertex to an igraph object, setting its name and ID, as well + as assigning values to certain attributes. It also attempts to find an + edge connecting the new vertex to a predefined "other side door" vertex, + based on attribute values. Args: - node (igraphNode): Used to represent a vertex to be added to an igraph - graph. It contains information such as the vertex name, ID, and type. + node (igraphNode): Passed the vertex to be added to the graph. """ self.graph.add_vertex(name=node.name, id=node.id, type=node.type) @@ -731,13 +744,13 @@ def insert_igraph_vertex(self, node): def insert_dsr_vertex(self, parent_name, node): # print("Inserting vertex", node["name"], node["type"]) """ - Inserts a new node into a graph, setting its parent node and attributes - based on the provided node object. + Inserts a new node into a graph, updating the parent node's attributes and + setting the new node's attributes based on the input node. Args: - parent_name (str): Used to specify the name of the parent node that - will hold the newly created node as a child. - node (Python): Passed as an instance of class `Node`. + parent_name (str): Used to specify the name of the parent node to which + the new node will be added. + node (dict): Passed as an attribute of a vertex. """ new_node = Node(agent_id=self.agent_id, type=node["type"], name=node["name"]) @@ -756,14 +769,13 @@ def insert_igraph_edge(self, edge): # Search for the origin and destination nodes in the graph """ - Adds an edge to an igraph object, providing the origin and destination - nodes and their respective attributes (translation and rotation) for the - edge. + Adds an edge to an igraph object based on the attributes of the given edge, + including the translation and rotation of the edge. Args: - edge (igraphEdge): An object containing information about an edge to - be added to an igraph graph, including its origin and destination - nodes, as well as rotation and translation attributes. + edge (igraphEdge): Passed as an instance of Igraph Edge class, which + contains attributes including origin, destination, rt translation, + and rotation. """ origin_node = self.graph.vs.find(id=edge.origin) @@ -779,15 +791,13 @@ def insert_dsr_edge(self, org, dest): # print("ORG::", org) # self.insert_dsr_vertex(dest) """ - Modifies an existing edge in a graph by adding new attributes "rt_translation" - and "rt_rotation_euler_xyz". It then inserts or assigns the modified edge - into the graph. + Adds an edge to the graph, connecting a node representing the origin to a + node representing the destination, with the given RT values and rotation. Args: - org (Agent): Represented as a Node object in the graph, containing - information such as id, name, and rotation. - dest (Agent): Used to represent the target node or edge of the graph, - which will be updated with the new RT value and rotation information. + org (instance): Representing the start node of the edge to be inserted. + dest (Agent): Passed as an argument to the function, representing the + destination node for which the edge is being inserted. """ if org is None: @@ -819,8 +829,10 @@ def insert_dsr_edge(self, org, dest): def draw_graph(self): """ - Clears the axis, layouts the graph using the Kamada-Kawai algorithm, and - plots arrows for edges. It also adds node labels and sets xy limits. + Draws the graph using Matplotlib, clearing the axis before laying out the + vertices and edges. It then plots each edge with an arrow pointing to its + tail node, and displays node names at their centers. Finally, it sets + limits on the x and y axes. """ self.ax.clear() @@ -849,16 +861,15 @@ def draw_graph(self): def check_element_room_number(self, node_id): """ - Retrieves the room ID associated with a given node ID through attribute - retrieval and returns it if successful, or -1 otherwise. + Retrieves the room ID associated with a given node ID by accessing the + `room_id` attribute of the node's attributes, and returns the room ID if + successful, or -1 otherwise. Args: - node_id (str): Used to identify the node whose room number needs to - be retrieved. + node_id (str): Used to retrieve a specific node from the graph. Returns: - int: The room ID of a given node's element or -1 if an error occurs - when trying to retrieve the room ID. + int: The room ID associated with a given node ID. """ node = self.g.get_node(node_id) @@ -871,16 +882,15 @@ def check_element_room_number(self, node_id): def check_element_level(self, node_id): """ - Within SpecificWorker, a subclass of GenericWorker, retrieves the element - level attribute from a given node and returns its value if found, or -1 - otherwise. It also checks the robot position and adjusts door connections - in the agent's internal graph if necessary. + Within the SpecificWorker class retrieves an element level attribute from + a given node and returns its value. If the attribute is not found, it + prints an error message and returns -1. Args: - node_id (str): Used as an identifier for a specific node in the graph. + node_id (str): Used as an identifier of a node within the graph. Returns: - integerints: The element level of the node with the given `node_id`. + integerints: The level of an element with the given node ID. """ node = self.g.get_node(node_id) @@ -928,13 +938,12 @@ def check_element_level(self, node_id): def generate_room_picture(self, room_node_id): """ - Within the `SpecificWorker` class retrieves and processes data related to - a specific room, including its ID, edges with a certain type, and translation - attribute. It also draws the room polygon and doors. + Retrieves the attributes of nodes connected by RT edges in a graph, and + then prints their origin and destination nodes, as well as any translation + attribute found. Args: - room_node_id (str): Used to identify the node representing the room - for which a picture needs to be generated. + room_node_id (str): Used to retrieve the node ID of the room to be drawn. """ room_node = self.g.get_node(room_node_id) @@ -973,12 +982,12 @@ def generate_room_picture(self, room_node_id): def insert_current_edge(self, room_id): # Insert current edge to the room """ - Inserts or assigns an edge in the graph with the given `room_id` as its - tail and the current agent's ID as its head. + Inserts or assigns an edge with specified properties to a graph, specifically + the current edge representing the worker's location. Args: - room_id (str): Passed as an argument to the function, representing the - ID of the current room that the agent is located in. + room_id (str): Used to specify the current room id for which the edge + is being created or updated. """ current_edge = Edge(room_id, room_id, "current", self.agent_id) @@ -995,17 +1004,53 @@ def update_node_att(self, id: int, attribute_names: [str]): def update_node(self, id: int, type: str): """ - Updates the state of an affordance node based on its ID and type. If the - active affordance node's state is "completed" and its active status is - false, it transitions to the "crossed" state. + Updates a node's type, checking if it's a door and inserting its corresponding + edge in the long-term graph. If the id is the affordance node's active id, + it checks if the node is completed and not active, and transitions to the + crossed state. Args: - id (int): Passed as an argument representing the unique identifier of - the node being updated. - type (str): Passed as an argument to the function, indicating the type - of node being updated. + id (int): Used to represent the unique identifier of a node in the graph. + type (str): Used to identify the node type, which determines the actions + performed on the node when the function is called. """ + 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) + print("RT DOOR", rt_door.attrs["rt_translation"].value, rt_door.attrs["rt_rotation_euler_xyz"].value) + 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) @@ -1025,13 +1070,14 @@ def update_edge(self, fr: int, to: int, type: str): # TODO: Posible problema si tocas el nodo room en la interfaz gráfica """ - Determines if there is a current edge connecting two nodes in a graph based - on specific conditions and updates the current edge if necessary. + Updates an edge based on its ID, type and node IDs. It checks if the + specified edge exists and is not the current edge, and sets it as the + current edge if conditions are met. Args: - fr (int): A reference to a specific node in the graph. - to (int): Referred to as the ID of a node in the graph. - type (str): Set to "RT". It represents the type of edge being updated. + fr (int): Representative of the ID of the node it originates from. + to (int): The ID of the destination node in the graph. + type (str): Set to "RT". """ 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: From c65c5a95587d234615e867af6a841f0c5ef8d9ad Mon Sep 17 00:00:00 2001 From: "komment-ai[bot]" <122626893+komment-ai[bot]@users.noreply.github.com> Date: Fri, 12 Jul 2024 12:07:50 +0000 Subject: [PATCH 06/10] Added comments to 37 functions across 2 files --- .komment/00000.json | 541 +++++++++--------- .komment/komment.json | 5 +- agents/g2o_agent/src/specificworker.py | 370 ++++++------ .../src/specificworker.py | 358 ++++++------ 4 files changed, 632 insertions(+), 642 deletions(-) diff --git a/.komment/00000.json b/.komment/00000.json index 109ae982..bb9415f6 100644 --- a/.komment/00000.json +++ b/.komment/00000.json @@ -241,192 +241,187 @@ "path": "agents/g2o_agent/src/specificworker.py", "content": { "structured": { - "description": "A class `Robot` that handles robot movement and mapping in a graph-based environment. It uses the `networkx` library for working with graphs and provides functions for updating node attributes, creating new nodes, deleting nodes, and updating edges. The code also includes functionality for handling RT (room transition) events and storing translation and rotation information for future use. Overall, the code is focused on tracking the movement of a robot in a graph-based environment and using that information to make decisions about how to navigate the space.", + "description": "An object `Odometry` that handles updating a graph and nodes within it based on various events such as node creations, deletions, updates, and edges updates. It also manages a queue for odometry data and has methods to update and delete nodes and edges based on their types. The code uses the `networkx` package for working with graphs.", "items": [ { - "id": "cedd1d2f-851b-87a9-c542-e4ba389373e2", + "id": "db065633-a20c-b4a3-b546-23abf1d410fe", "ancestors": [], - "description": "Performs specific tasks related to a worker robot's navigation and localization in an unknown environment, such as updating the graph, getting displacement, computing covariance matrix, visualizing G2O real-time, and more.", + "description": "Manages a graph, updates node attributes, and sets edges based on robot odometry data and room changes. It also handles RT edge tracking and stores translation and rotation values for later use.", "attributes": [ { "name": "Period", - "type_name": "float", - "description": "Used to control the time interval between consecutive calls to the `update_worker` method, which is responsible for updating the robot's state based on the received data. The value of `Period` determines how often the worker will be called, with larger values resulting in slower updates but potentially reducing the load on the system." + "type_name": "instance", + "description": "Used to control the update rate of the graph. It sets the time interval between successive updates in milliseconds, which can be used to optimize the performance of the worker." }, { "name": "agent_id", "type_name": "int", - "description": "Used as a unique identifier for the agent's id." + "description": "Used to identify the agent that owns the robot. It is used to determine which agent's graph should be updated when a new edge is added or an existing edge is modified." }, { "name": "g", - "type_name": "undirected", - "description": "Used to store the graph representing the environment. It contains nodes and edges that represent the obstacles, walls, and other features of the environment." + "type_name": "Graph", + "description": "Used for representing the robot's environment through a graph, with nodes representing rooms and edges representing the movement of the robot between them." }, { "name": "startup_check", "type_name": "QTimersingleShot", - "description": "Used to check for the application's quit after a certain time interval, which is 200 milliseconds in this case." + "description": "Used to check if the user wants to quit the application after a certain period of time has passed since the last update. It schedules a single shot event every 200 milliseconds to check if the user wants to quit." }, { "name": "rt_api", - "type_name": "str", - "description": "100% sure to be a valid ROS topic name for receiving real-time data from a robotic arm." + "type_name": "instance", + "description": "Used to store information related to the RT (Real-time) module, such as the last time it was accessed and the translation and rotation values set for the current room." }, { "name": "inner_api", "type_name": "instance", - "description": "Used to store a reference to the inner API of the worker, which allows for direct communication with the worker's inner workings." + "description": "A method that updates the graph with new nodes, edges, or attributes based on the inner loop of the worker." }, { "name": "odometry_node_id", "type_name": "int", - "description": "Used to identify the node in the graph that corresponds to the robot's odometry information. It is used to store the odometry data in the graph and for visualization purposes." + "description": "Used to store the ID of the node representing the robot's position in the graph." }, { "name": "odometry_queue", - "type_name": "3element", - "description": "Used to store the odometry information of the robot in a First-In-First-Out (FIFO) queue. It stores the advance, lateral, and angular displacement of the robot at each time step, which are computed using the G2O optimization algorithm." + "type_name": "list", + "description": "Used to store the current odometry data of the robot, including its advance speed, side speed, and angular speed, which are updated at a rate of 200 Hz." }, { "name": "last_odometry", - "type_name": "3element", - "description": "Used to store the last known odometry information of the robot, which is used in the computation of the displacement and covariance matrix." + "type_name": "int", + "description": "Used to store the time at which the worker last received odometry data from the environment. It is updated every time a new odometry message is received, allowing the worker to determine how long ago it received the message." }, { "name": "g2o", - "type_name": "Optimizer", - "description": "Used to store the Gauss-Newton optimizer for graph matching. It is used to compute marginals \nand to update the position of vertices in the graph." - }, - { - "name": "visualizer", - "type_name": "Visualizer", - "description": "Used to visualize the graph and edges in real-time during the RT algorithm execution. It provides a function to update the visualization and can be used to display the graph and edges in different formats, such as 3D or 2D." + "type_name": "Graph", + "description": "Used to store the graph data of the environment. It contains the nodes, edges, and attributes of the graph." }, { "name": "odometry_noise_std_dev", - "type_name": "float", - "description": "0.1 by default, representing the standard deviation of noise in the odometry measurements. It helps to control the level of random fluctuations in the robot's motion." + "type_name": "floatingpoint", + "description": "Used to specify the standard deviation of noise added to the odometry measurements for training purposes." }, { "name": "odometry_noise_angle_std_dev", "type_name": "float", - "description": "0.8 by default, representing the standard deviation of the angle noise added to the odometry measurements for more accurate pose estimation." + "description": "0.1 by default, representing the standard deviation of the noise added to the robot's angle readings during odometry estimation." }, { "name": "measurement_noise_std_dev", "type_name": "float", - "description": "Used to represent the standard deviation of the noise present in the robot's measurements. It affects how much the robot's estimate deviates from the true position." + "description": "0.1 by default, representing the standard deviation of measurement noise for the robot's sensors. It determines how much noise is added to the measured positions and orientations of the robot in the simulation." }, { "name": "last_room_id", "type_name": "int", - "description": "Used to store the room ID of the last room that was processed by the worker before its initialization was changed." + "description": "Used to store the last room ID seen by the worker before changing rooms." }, { "name": "actual_room_id", "type_name": "int", - "description": "Used to keep track of the current room ID that the worker is in during its navigation. It is updated whenever the worker moves from one room to another." + "description": "Used to store the current room ID of the agent during navigation." }, { "name": "elapsed", - "type_name": "float", - "description": "Used to keep track of the time elapsed since the last successful message received from the robot. It is updated every time a message is processed, and its value represents the time spent processing the message." + "type_name": "int", + "description": "Used to keep track of the time elapsed since the last call to the `startup_check()` function, which is used to check if the application should be closed after a certain period of inactivity." }, { "name": "room_initialized", "type_name": "bool", - "description": "Used to keep track of whether the worker has initialized the room or not. It is set to False when the worker first enters a new room, and to True when it exits that room." + "description": "Used to track whether the current room has been initialized or not, it's set to False when a new room is entered and True otherwise." }, { "name": "iterations", "type_name": "int", - "description": "0-indexed, indicating the number of iterations (or passes) that the worker has performed on the graph. It is updated each time the worker processes a new edge or node in the graph." + "description": "0-indexed, indicating the number of iterations of the worker's tasks it can perform before finishing." }, { "name": "hide", "type_name": "str", - "description": "Used to indicate whether the worker should be hidden or not. When set to \"True\", the worker will be hidden from the main window; otherwise, it will be displayed." + "description": "Used to determine whether a node or edge should be hidden from the graph. It allows you to specify which nodes or edges to hide based on their ID, label, or other attributes." }, { "name": "init_graph", - "type_name": "Python", - "description": "Used to indicate whether the graph has been initialized or not. It is set to `True` when the graph is first constructed and then reset to `False` when the graph is updated." + "type_name": "instance", + "description": "Used to keep track of whether the graph has been initialized or not, it's set to `True` when the graph is first created and `False` otherwise." }, { "name": "current_edge_set", "type_name": "bool", - "description": "Used to keep track of whether the current edge set has been updated with a new RT translation or rotation. It is set to True when an edge set is updated with a new RT translation or rotation, and False otherwise." + "description": "Used to track whether the current edge being processed is part of the RT set or not." }, { "name": "first_rt_set", - "type_name": "Python", - "description": "Initialized to `True` when the agent first sets a translation and rotation for the RT task, indicating that it is the first time this has happened in the simulation." + "type_name": "bool", + "description": "Set to True when a new RT (remote transmission) set is encountered for the first time during the simulation, indicating that the worker has entered a new room or area." }, { "name": "translation_to_set", "type_name": "3D", - "description": "Used to store the translation of the RT object to the set of corners of the room." + "description": "Used to store the translation of a robot's end effector when it enters a specific set. It is used in conjunction with the \n`rotation_to_set` attribute to determine the full pose of the robot when it enters the set." }, { "name": "rotation_to_set", "type_name": "3D", - "description": "Used to store the rotation of a robot relative to its set position." + "description": "Used to store the rotation of the robot relative to its set position." }, { "name": "room_polygon", - "type_name": "3D", - "description": "Used to represent the room where the robot is currently located. It stores a list of 3D vertices that define the shape of the room." + "type_name": "QPolygon", + "description": "Used to store the polygon representation of a room in the environment." }, { "name": "security_polygon", - "type_name": "list", - "description": "Used to store a polygon representing the security area around the worker's target location. It is used to check if the worker's path intersects with any obstacles or security areas, ensuring the worker's safety during its motion." + "type_name": "QPolygon", + "description": "Used to store the security polygon of a specific worker in a graph, which is used for collision detection and response in robotics." }, { "name": "initialize_g2o_graph", "type_name": "instance", - "description": "Responsible for initializing a Graph2O graph object to represent the environment, which is used for computing the robot's pose and motion." + "description": "Used to create a Graph2O graph from a robot's observation, which is then used for optimization. It initializes the graph by adding nodes and edges based on the robot's observation and sets up the necessary attributes for optimization." }, { "name": "rt_set_last_time", "type_name": "int", - "description": "Used to keep track of the time since the last RT set was received. It is used to determine when to send a new RT set." + "description": "Used to track the time since the last RT set was received for a given agent ID. It is used in the `update_edge()` function to determine if enough time has passed since the last RT set for the robot to consider setting a new translation and rotation." }, { "name": "rt_time_min", "type_name": "float", - "description": "Used to set a minimum time interval between RT sets. It serves as a threshold for determining when a new RT set should be initiated based on the agent's movement." + "description": "Set to the minimum time interval between two RT sets that are considered as a new RT set. It is used to determine when to reset the translation and rotation to set values in the `update_edge()` method." }, { "name": "last_update_with_corners", "type_name": "int", - "description": "Used to store the last time corners were updated. It's used in conjunction with other attributes to maintain a consistent update schedule for corners." + "description": "Used to keep track of when the worker last updated its graph with corners. It is set to the current time whenever the `update_node`, `update_edge`, or `delete_edge` methods are called, and is used to determine when the graph has changed significantly enough to warrant updating the robot's state." }, { "name": "timer", "type_name": "QTimer", - "description": "Used to schedule a call to the `QApplication.instance().quit()` function every 200 milliseconds, indicating that the worker should shut down." + "description": "Used to schedule a call to the `QApplication.instance().quit()` function after 200 milliseconds, which means that the worker will quit after a certain amount of time has passed." }, { "name": "compute", - "type_name": "lambda", - "description": "Used to compute the marginal probability of the worker's variables given the observed data. It takes an index `i` as input and returns a tuple containing the probability of each variable in the worker's graph given the observed data, as well as the gradient of the probability with respect to the variable's value." + "type_name": "instance", + "description": "Used to handle updates for node attributes. It takes two arguments: `id` which is the id of the node to be updated, and `attribute_names` which is a list of strings representing the names of the attributes to be updated." }, { "name": "update_node_att", - "type_name": "QMetaObjectAttribute", - "description": "Used to update the attributes of a node in the graph when a new attribute is received from the ROS bag." + "type_name": "event", + "description": "Called when a new node attribution is received from the graph. It updates the robot's odometry queue with the current advance, side, and angular speed and sets the `room_initialized` flag to false if the room ID changes." }, { "name": "update_edge", - "type_name": "str", - "description": "Used to update the edge attributes based on the edge type. It takes three parameters: `fr`, `to`, and `type`, which are the index of the current edge, the index of the next edge, and the edge type, respectively. The method updates the edge attributes based on the edge type and sets the `current_edge_set` attribute to `True`." + "type_name": "update_edge", + "description": "Used to update the attributes of an edge in a graph based on its type, such as \"current\" or \"RT\". It sets the translation and rotation of the robot based on the RT set." }, { "name": "update_edge_att", - "type_name": "edge", - "description": "Used to update the attributes of an edge in the graph." + "type_name": "str", + "description": "Defined as a method that updates the attributes of an edge in the graph based on a specific edge type and a list of attribute names." } ], "name": "SpecificWorker", @@ -438,25 +433,25 @@ "comment": null }, "item_type": "class", - "length": 424, + "length": 416, "docLength": null }, { - "id": "ba4c99e5-6e75-fe9c-5c42-896fd963bd46", + "id": "736bb328-b4ca-7b8b-5e4f-cb66525a0d81", "ancestors": [ - "cedd1d2f-851b-87a9-c542-e4ba389373e2" + "db065633-a20c-b4a3-b546-23abf1d410fe" ], - "description": "Initializes an instance of the SpecificWorker class by setting up various components such as g2o graph, visualizer, and timers. It also performs startup checks and connects signals for node and edge updates.", + "description": "Initializes a SpecificWorker object, setting its properties and connecting to signals for updating node attributes, edges, and edge attributes.", "params": [ { "name": "proxy_map", "type_name": "dict", - "description": "Used to store mapping information between the original graph and the transformed graph for DSR algorithm implementation." + "description": "Used to specify a mapping from real-world coordinates to virtual coordinates for the robot's movement." }, { "name": "startup_check", - "type_name": "Optionalbool", - "description": "Used to check for any errors in the graph during initialization." + "type_name": "bool", + "description": "Used to run an initialization check on the graph when the agent starts up." } ], "returns": null, @@ -469,15 +464,15 @@ "comment": null }, "item_type": "method", - "length": 56, + "length": 54, "docLength": null }, { - "id": "9f5049cf-1b33-95b0-d74c-970e8a2091b1", + "id": "6c6431d9-1393-d3b9-5b46-3847c0c49dcb", "ancestors": [ - "cedd1d2f-851b-87a9-c542-e4ba389373e2" + "db065633-a20c-b4a3-b546-23abf1d410fe" ], - "description": "Performs RT odometry computation based on sensor data and updates the Graph2O graph with landmarks, affordances, and RT edges. It also maintains counters for valid corners and doors, and rotates the robot if necessary.", + "description": "In the `SpecificWorker` class implements a robot's movement using ROS navigation stack, computing the robot's position and orientation based on the previous odometry data and adding it to the graph.", "params": [], "returns": null, "name": "compute", @@ -489,19 +484,19 @@ "comment": null }, "item_type": "method", - "length": 102, + "length": 96, "docLength": null }, { - "id": "7d619d37-ef8e-0995-9f40-680ea469e08e", + "id": "1f7b5f1f-be86-5eb8-b24b-1aac542cd83a", "ancestors": [ - "cedd1d2f-851b-87a9-c542-e4ba389373e2" + "db065633-a20c-b4a3-b546-23abf1d410fe" ], - "description": "Initializes a Graph2O graph for a specific robot and room by:\n\n* Extracting landmarks from the robot's point cloud\n* Calculating nominal corners based on landmarks and room geometry\n* Adding fixed poses to the g2o graph for the robot and doors in the room.", + "description": "Initializes a Graph2Online (G2O) graph for a specific robot's odometry data, by adding nominal corners and fixed poses to the graph based on room and door nodes in the environment map.", "params": [], "returns": { "type_name": "bool", - "description": "1 if the initialization of the g2o graph was successful, and 0 otherwise." + "description": "1 if the function was able to initialize the g2o graph successfully, and 0 otherwise." }, "name": "initialize_g2o_graph", "location": { @@ -516,21 +511,21 @@ "docLength": null }, { - "id": "cc39009c-98a2-8683-9e40-55a39ba52731", + "id": "fb17fb15-9dd2-2aac-e149-6ea477cc49e5", "ancestors": [ - "cedd1d2f-851b-87a9-c542-e4ba389373e2" + "db065633-a20c-b4a3-b546-23abf1d410fe" ], - "description": "Computes and updates the displacement values (lateral, forward, and angular) of a robot based on its odometry data, using a moving average of recent positions to smooth out the motion.", + "description": "Calculates the displacement of an agent based on its odometry data, taking into account advancement, lateral movement, and angular movement.", "params": [ { "name": "odometry", - "type_name": "3element", - "description": "An instance of the `Odometry` class, representing the robot's position and velocity over time." + "type_name": "dict", + "description": "Passed as an argument to the function, containing the odometric data of the vehicle at each time step, including the position, velocity, and timestamp." } ], "returns": { "type_name": "3element", - "description": "The lateral displacement (in meters), the forward displacement (in meters) and angular displacement (in radians)." + "description": "3-D vector representing the displacement of a robot's end effector in terms of lateral, forward, and angular displacements." }, "name": "get_displacement", "location": { @@ -545,21 +540,21 @@ "docLength": null }, { - "id": "90a11b67-7e49-1f85-f542-5b75590854cc", + "id": "3a44433e-c164-0bb0-a048-743c75fa20f0", "ancestors": [ - "cedd1d2f-851b-87a9-c542-e4ba389373e2" + "db065633-a20c-b4a3-b546-23abf1d410fe" ], - "description": "Computes the covariance matrix for a given vertex in a graph, using an optimization algorithm to compute the marginals of the vertices and then constructing the covariance matrix.", + "description": "Computes the covariance matrix of a set of vertices in a graph, using the gradient descent optimizer from the `g2o` library. It returns the computed covariance matrix and whether it was successfully computed or not.", "params": [ { "name": "vertex", - "type_name": "g2oVertex", - "description": "Used to represent a vertex in the graph." + "type_name": "g2overtex", + "description": "Used to represent a specific vertex in a graph." } ], "returns": { - "type_name": "2tuple", - "description": "A pair of a boolean result and a covariance matrix." + "type_name": "tuple", + "description": "2-element tuple containing two values: (`covariances_result`, `covariances`)." }, "name": "get_covariance_matrix", "location": { @@ -574,16 +569,16 @@ "docLength": null }, { - "id": "6eecaaa5-da88-1192-224a-b19526aa0376", + "id": "889d2404-a66a-f3b6-8e47-4705c72d6823", "ancestors": [ - "cedd1d2f-851b-87a9-c542-e4ba389373e2" + "db065633-a20c-b4a3-b546-23abf1d410fe" ], - "description": "Within the `SpecificWorker` class loads a G2O file, visualizes the vertices and edges of the graph in a 3D scatter plot, and updates the plot in real-time as new measurements are received.", + "description": "Visualizes the 3D positions of vertices and edges in a G2O file in real-time, using matplotlib. It loads the G2O file, calculates the positions of the vertices and edges, and plots them on a 3D scatter plot.", "params": [ { "name": "optimizer", "type_name": "instance", - "description": "An object that loads G2O files and provides access to their vertices and edges for visualization." + "description": "Used to store an instance of the G2O optimizer class, which is responsible for minimizing the energy of the system." } ], "returns": null, @@ -600,21 +595,21 @@ "docLength": null }, { - "id": "25fc8d71-2815-d7be-5f44-ddf8121e9654", + "id": "dade48c4-b3b7-33be-694f-984e420abf6c", "ancestors": [ - "cedd1d2f-851b-87a9-c542-e4ba389373e2" + "db065633-a20c-b4a3-b546-23abf1d410fe" ], - "description": "Updates an attribute of a node in a graph, specifically the odometry node, by appending its current position and velocity to a queue for processing.", + "description": "Updates the attributes of a specific node in a graph, based on the current time and other factors.", "params": [ { "name": "id", "type_name": "int", - "description": "Passed as an argument to the function, representing the unique identifier for the node being updated." + "description": "Used to represent the unique identifier of the node being updated, specifically the odometry node ID." }, { "name": "attribute_names", "type_name": "[str]", - "description": "A list of names of attributes to update on the node associated with the given ID." + "description": "An array of names of attributes to be updated on the node." } ], "returns": null, @@ -631,21 +626,21 @@ "docLength": null }, { - "id": "003e6136-27e7-beb3-ab41-3f3c52ff0bb3", + "id": "ae1da25f-ceee-21ae-874c-7905cb623444", "ancestors": [ - "cedd1d2f-851b-87a9-c542-e4ba389373e2" + "db065633-a20c-b4a3-b546-23abf1d410fe" ], - "description": "Updates a node's type, but only if the type is \"corner\". If it is, the function also checks if the \"room\" node has been initialized and sets the `init_graph` attribute to `True`.", + "description": "Updates an unspecified node with an ID and a type. If the type is \"corner\", it initializes a room graph if necessary. Otherwise, it does nothing.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to represent the unique identifier of a node." + "description": "Used to represent the unique identifier for the node being updated." }, { "name": "type", "type_name": "str", - "description": "Used to indicate the type of node being updated, specifically whether it is a corner node or not." + "description": "Defined as \"corner\"." } ], "returns": null, @@ -662,11 +657,11 @@ "docLength": null }, { - "id": "7ae77879-8a05-aa85-404f-29e9171fb57a", + "id": "8497841c-0cf0-3ca6-7a4c-1ada9b487ccd", "ancestors": [ - "cedd1d2f-851b-87a9-c542-e4ba389373e2" + "db065633-a20c-b4a3-b546-23abf1d410fe" ], - "description": "Deletes a node from a list maintained by a `SpecificWorker` instance, setting a flag to indicate that the list has been modified.", + "description": "Deletes a node from a data structure managed by a `SpecificWorker` subclass of `GenericWorker`.", "params": [ { "name": "id", @@ -688,26 +683,26 @@ "docLength": null }, { - "id": "244f7983-496d-92a0-dc48-2e8b20b80070", + "id": "f9896a12-dfdf-789b-f14f-bdb730087303", "ancestors": [ - "cedd1d2f-851b-87a9-c542-e4ba389373e2" + "db065633-a20c-b4a3-b546-23abf1d410fe" ], - "description": "Updates the current room ID and RT translation and rotation when the agent moves from one room to another, and sets the `current_edge_set` variable to `True`.", + "description": "Updates the room ID and sets the `current_edge_set` attribute based on the type of edge received, and also performs RT translation and rotation if necessary.", "params": [ { "name": "fr", "type_name": "int", - "description": "A reference to a node in the graph represented by the class instance." + "description": "Representative of the starting vertex of an edge in a graph." }, { "name": "to", "type_name": "int", - "description": "Used as an index to access the target node of an edge in the graph." + "description": "Used to represent the ID of the next node in the graph that the edge will be attached to." }, { "name": "type", "type_name": "str", - "description": "Used to identify the edge type, which can be either \"current\" or \"RT\"." + "description": "Used to indicate the type of edge being updated, either \"current\" or \"RT\"." } ], "returns": null, @@ -724,26 +719,26 @@ "docLength": null }, { - "id": "7c04e666-9a89-a691-0247-1180243804a0", + "id": "338c7aa8-6757-9cbe-9845-75ded7f5c60a", "ancestors": [ - "cedd1d2f-851b-87a9-c542-e4ba389373e2" + "db065633-a20c-b4a3-b546-23abf1d410fe" ], - "description": "Deletes an edge from a graph based on its FID (fr), TID (to), and edge type (type).", + "description": "Deletes an edge from a graph, specified by its ID and type.", "params": [ { "name": "fr", "type_name": "int", - "description": "Used as the index of the edge to be deleted." + "description": "Passed as an argument to the function with the value of 123." }, { "name": "to", "type_name": "int", - "description": "Used to represent the destination node ID for edge deletion." + "description": "Used to specify the target vertex ID for edge deletion." }, { "name": "type", "type_name": "str", - "description": "Used to specify the edge type that should be deleted." + "description": "Used to specify the edge type to be deleted, which can be either 'weighted' or 'unweighted'." } ], "returns": null, @@ -1163,137 +1158,137 @@ "path": "agents/long_term_spatial_memory_agent/src/specificworker.py", "content": { "structured": { - "description": "A class called `DSR` that implements a graph-based representation of a robot's environment and its interactions with objects in that environment. The `DSR` class has several methods for updating and manipulating the graph, including inserting new nodes or edges, updating node attributes, deleting nodes or edges, and more. These methods use various high-level packages such as `QApplication`, `QTimer`, `QDebug`, and `pickle` to perform their operations.\n\nOverall, the code defines a system for representing complex environmental interactions between a robot and its surroundings, using graph theory concepts and Python's high-level packages.", + "description": "a class `Room` that represents a room in a graphical user interface (GUI) and performs various operations on it. The class has several methods:\n\n* `insert_room_node`: Inserts a new room node into the graph.\n* `update_edge`: Updates an edge in the graph, setting its type to \"current\" if necessary.\n* `delete_node`: Deletes a room node from the graph.\n* `update_edge_att`: Updates an edge attribute in the graph.\n* `delete_edge`: Deletes an edge from the graph.\n\nThe code uses the `igraph` package to interact with the graph, and the `pickle` package for serializing and deserializing the graph. The class also has some console print statements to display messages during execution.", "items": [ { - "id": "8e2d2981-1343-51a0-0a43-35e06cb1288a", + "id": "d14b1fa8-9d9a-61a7-184c-3f4e98189154", "ancestors": [], - "description": "Manages a robot's interactions with its environment, including moving through doors, creating new rooms, and updating node attributes. It also generates images of rooms and handles long-term graph management.", + "description": "Manages a graph representing a robot's movement and interactions with its environment, performing various operations such as inserting, updating, and deleting nodes and edges, as well as tracking the robot's state.", "attributes": [ { "name": "Period", "type_name": "str", - "description": "Used to set a fixed time interval for the worker to perform its function, allowing to schedule the work in a more organized manner." + "description": "Used to store the current time period in which the worker is active, allowing for more efficient handling of tasks during different time slots." }, { "name": "agent_id", "type_name": "int", - "description": "Used to identify the worker as a specific agent, such as a robot or human, within the framework." + "description": "Used as the ID of the agent that owns or manipulates the graph, which could be a robot or a human operator." }, { "name": "g", - "type_name": "igraph", - "description": "A weighted graph that represents the environment of the agent. It stores the nodes (rooms) and edges between them, which represent the doors connecting the rooms. The graph is used to determine the robot's movement and interactions with its environment." + "type_name": "igraphGraph", + "description": "Used for manipulating the graph in various methods like `insert_igraph_vertex`, `insert_igraph_edge`, etc." }, { "name": "update_node", - "type_name": "str", - "description": "Used to update a specific node in the graph based on its ID. It takes one argument, which is the type of update (either \"door\" or \"room\"), and performs different actions depending on the update type." + "type_name": "int", + "description": "Used to update a node's attributes based on its ID. It takes two arguments: `id` (int) and `type` (str), where `id` is the node's ID to be updated and `type` represents the type of update (e.g., \"door\", \"room\")." }, { "name": "update_edge", - "type_name": "str", - "description": "Used to update the edge type of a specific edge in the graph. It takes two arguments: the first is the id of the starting node, and the second is the id of the ending node, as well as an optional third argument which is the new edge type to be set for that edge." + "type_name": "int", + "description": "Used to update the edge of a robot moving from one node to another with a certain type (current) when there is no current edge and a room node exists." }, { "name": "startup_check", - "type_name": "QTimersingleShot", - "description": "Used to check for possible termination of the worker after a specific timeout period of 200 milliseconds." + "type_name": "str", + "description": "Used to check if the worker is in startup mode or not. It is set to \"true\" when the worker starts up and \"false\" otherwise." }, { "name": "rt_api", - "type_name": "instance", - "description": "Used for accessing the RT API of the environment to get information about the robot's position, orientation, and other relevant data." + "type_name": "str", + "description": "Represented as a string value indicating which RT (Real-Time) affordance to use." }, { "name": "inner_api", - "type_name": "internal", - "description": "Used for communication between different agents in a multi-agent environment, allowing them to exchange information and coordinate their actions." + "type_name": "instance", + "description": "A Python method that returns the inner API of the worker. It is used to define the functionality of the worker and its interactions with other components of the system." }, { "name": "robot_name", "type_name": "str", - "description": "Used as the ID of the robot in the internal graph, which is generated based on the agent's name." + "description": "Used to store the name of the robot being controlled by the worker." }, { "name": "robot_id", "type_name": "int", - "description": "Used to represent the unique identifier of the robot agent in the environment." + "description": "Used to identify the robot node in the graph." }, { "name": "last_robot_pose", - "type_name": "3D", - "description": "Used to store the last known position of the robot, which can be used to determine the robot's motion and interaction with its environment." + "type_name": "IGraph", + "description": "Used to store the last known pose of the robot, which can be used for debugging purposes or to track the robot's movement." }, { "name": "robot_exit_pose", - "type_name": "str", - "description": "3D pose representation of the robot when it exits a room. It is used to check if the robot has reached its destination and to generate an image of the room." + "type_name": "RT", + "description": "A dictionary containing the pose (position and orientation) of the robot when it exits the room through the door." }, { "name": "state", "type_name": "str", - "description": "Used to store the current state of the worker (either \"idle\", \"busy\", or \"crossed\") indicating whether the worker is available or not for new tasks." + "description": "Used to store the current state of the worker, which can be either \"idle\", \"working\", or \"crossed\"." }, { "name": "affordance_node_active_id", "type_name": "int", - "description": "Used to store the ID of the affordance node that indicates when a robot reaches a certain affordance, it will stop moving and wait for further instructions." + "description": "Used to store the ID of the affordance node that is currently active. It is used to check if the affordance node is completed and not active, and to switch to the crossed state when it is completed and not active." }, { "name": "exit_door_id", "type_name": "int", - "description": "13 by default, representing the index of the door node that marks the exit of a room in the graph. It is used to identify the door through which the robot exits a room when it needs to navigate to another room." + "description": "1234, which represents the ID of the door node that serves as the exit of a room." }, { "name": "room_exit_door_id", "type_name": "int", - "description": "0-indexed. It represents the ID of the door connecting a room to the environment. It is used to identify the door through which the robot can exit a room." + "description": "Used to represent the ID of the door node that leads from a room to the outside world in the graph." }, { "name": "enter_room_node_id", "type_name": "int", - "description": "Used to store the ID of the room node that the worker enters after crossing the door." + "description": "0 by default, representing the ID of the room node that the worker enters when it completes its task." }, { "name": "graph", "type_name": "igraphGraph", - "description": "Used to store the agent's internal graph, which represents the environment and the robot's movements." + "description": "Used for storing and manipulating the graph represented by the worker. It contains the nodes, edges, and other attributes of the graph." }, { "name": "vertex_size", "type_name": "int", - "description": "16 by default, which means that each vertex in the graph has a size of 16 pixels in the internal graph representation." + "description": "Used to store the size of the vertex (node) in the graph, indicating the number of attributes associated with each node." }, { "name": "not_required_attrs", "type_name": "list", - "description": "Used to store a list of attribute names that are not required for the worker's functionality, but may be useful for debugging or other purposes." + "description": "Used to keep track of a list of attributes that are not required for the worker's functionality, but can be useful for debugging or other purposes." }, { "name": "long_term_graph", "type_name": "igraphGraph", - "description": "Used for storing the long-term graph of the environment, which is different from the short-term graph used for navigation." + "description": "Used for storing the long-term graph of the environment, which is updated when certain events occur like inserting or deleting nodes." }, { "name": "global_map", "type_name": "igraphGraph", - "description": "Used to store the global map of the environment. It is used to represent the entire environment and its connections, rather than just the local area around a single agent or node." + "description": "Used to store the global map of the environment, which is a graph representation of the environment that includes all nodes and edges." }, { "name": "insert_current_edge", - "type_name": "Edge", - "description": "Used to insert a new edge in the graph with the current room as the source and the robot as the destination, with the \"current\" edge type." + "type_name": "igraphEdge", + "description": "Used to insert a new edge into the graph with the given source and target nodes, and the specified edge type." }, { "name": "timer", - "type_name": "QTimer", - "description": "Used to schedule the worker's update loop to run after a certain delay (200 milliseconds in this case)." + "type_name": "int", + "description": "Set to the number of milliseconds since the epoch (January 1, 1970, 00:00:00 UTC) when the worker was created. It is used to track the elapsed time for the worker's tasks." }, { "name": "compute", "type_name": "instance", - "description": "Used to compute the worker's output for a given input." + "description": "Used to compute the shortest path between two nodes in the graph based on the RT model." } ], "name": "SpecificWorker", @@ -1305,25 +1300,25 @@ "comment": null }, "item_type": "class", - "length": 568, + "length": 572, "docLength": null }, { - "id": "241962dc-0914-a79d-9b40-5909c260c9ce", + "id": "768be6cd-7a26-04b8-5c48-d827e00b1c97", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Initializes an object of the `SpecificWorker` class, setting its graph, period, and other properties.", + "description": "Initializes an instance of the SpecificWorker class, setting up graph connections and variables for storing information about a robot's environment and state.", "params": [ { "name": "proxy_map", "type_name": "igraphGraph", - "description": "Used to initialize the graph object for the specific worker's long-term spatial memory." + "description": "Used to represent the graph mapping between nodes and objects, allowing for efficient object manipulation." }, { "name": "startup_check", "type_name": "bool", - "description": "Used to check if the graph has been modified since the last startup. If set to True, it will call the `startup_check` method, otherwise it will skip it." + "description": "Used to check if the agent has already been started." } ], "returns": null, @@ -1340,16 +1335,16 @@ "docLength": null }, { - "id": "6bfee08c-8e0e-7abf-324a-8ea14b6e006b", + "id": "b75c11a0-d78b-c099-0542-820638964df9", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Sets parameters for an object, removes a self-edge from a room, stores ID of the exit door, names both doors with the other side door attribute, and reads entrance door node to add other side door attributes.", + "description": "Sets the parameters of an object, then removes a self-edge from a room and stores the ID of an exit door in a variable. It also assigns attributes to both doors in the new room.", "params": [ { "name": "params", "type_name": "object", - "description": "Passed to set parameters for an object of class `Room`." + "description": "Used to store any relevant data or configuration for the function's operation." } ], "returns": { @@ -1369,11 +1364,11 @@ "docLength": null }, { - "id": "ed2b32ce-b574-b9a6-4c4b-a7dafa3d6b90", + "id": "bd530f7b-1d60-7699-5c46-38a9649a60a7", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Determines the current state of the worker and performs the appropriate action based on that state, including idling, crossing, crossing, initializing rooms, knowing rooms, initializing doors, storing the graph, and removing nodes.", + "description": "Determines the current state of a worker and performs the appropriate actions based on that state, including idle, crossing, crossed, initializing room, known room, initializing doors, storing graph, and removing.", "params": [], "returns": null, "name": "compute", @@ -1389,11 +1384,11 @@ "docLength": null }, { - "id": "250b555a-b6a6-759f-324b-d064b390a847", + "id": "75ed2888-b3fb-1cbf-ce45-8c960f6d2330", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Performs affordance-based navigation for an autonomous robot, specifically:\n\n1. Identifies active affordance nodes in the short-term graph.\n2. Checks if any \"current\" edge exists and if so, identifies the door node leading to the next room.\n3. Computes the final robot pose and door pose in the new room reference frame using the inner API.", + "description": "Performs the following tasks:\n\n1. Identifies active affordance nodes in the short-term graph.\n2. Checks if any \"current\" edge exists in the short-term graph.\n3. If a door is found, computes its pose in the long-term graph and associates it with the current room.", "params": [], "returns": null, "name": "idle", @@ -1409,11 +1404,11 @@ "docLength": null }, { - "id": "c19737e3-8045-f7b9-134c-d9f08ec98f43", + "id": "09e1943c-c650-459e-6f49-e8c99cf5b03f", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Determines whether a node represents an exit door and, if so, updates the state of the `SpecificWorker` instance to reflect this information.", + "description": "Determines the current room based on an active affordance node and updates the state machine accordingly. If the node has no parent, it sets the exit door ID to the value of the active affordance node's parent attribute.", "params": [], "returns": null, "name": "crossed", @@ -1429,11 +1424,11 @@ "docLength": null }, { - "id": "5e3379bd-db81-d1a5-8149-2cb24d34ac7c", + "id": "d5bde5e7-2579-51a6-ca41-9f515472bf25", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Determines and remembers the room node ID that the worker will enter after leaving the current room, and updates the state to \"initializing doors\".", + "description": "1) identifies room nodes in the graph, 2) sets the `enter_room_node_id` field, and 3) enters the \"initializing doors\" state.", "params": [], "returns": null, "name": "initializing_room", @@ -1449,11 +1444,11 @@ "docLength": null }, { - "id": "11cbfdf2-e11b-e0aa-4947-9344b9dd510a", + "id": "0abb8428-4ef9-1099-494a-164dd3beb89f", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Within the SpecificWorker class updates the RT object's position and rotation based on the last known affordance pose, taking into account the robot's movement and the associated door found in the global map.", + "description": "Within the `SpecificWorker` class defines how a robot navigates through a graph representation of a maze to reach an objective room while avoiding obstacles and taking into account the associated door's rotation.", "params": [], "returns": null, "name": "known_room", @@ -1469,11 +1464,11 @@ "docLength": null }, { - "id": "67025356-2f92-3c84-ef43-e9f81d799e94", + "id": "11fcaca4-6eb8-0f92-fe46-b59555cb21f2", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "1) finds edges leading to the exit door, 2) checks if any match edges exist, and 3) updates node attributes and associations for doors involved.", + "description": "Identifies doors connected to a specified exit door and associates them with each other, updating node attributes and the state of the worker.", "params": [], "returns": null, "name": "initializing_doors", @@ -1489,21 +1484,21 @@ "docLength": null }, { - "id": "cb77cc09-6f32-8aa9-244d-8bd8ff15aafd", + "id": "64841fe3-a4e4-5e90-864a-db8fe9332e9f", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Adds an edge to a graph between two nodes representing doors, and sets their \"other side door name\" and \"connected room name\" attributes based on the input door names.", + "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 name of a door node in the graph." + "description": "A string representing the name of the first door to be associated with another door in the graph." }, { "name": "door_2", "type_name": "str", - "description": "Represented as a list of two elements, where each element represents the name of a door node in the graph." + "description": "Represented as an igraph vertex object, which contains information about the door node to be associated with the first door node passed as input." } ], "returns": null, @@ -1520,11 +1515,11 @@ "docLength": null }, { - "id": "8cdc79dc-b155-0387-ae48-8f95fac0413f", + "id": "d14e9d2c-fd3b-9c95-d54d-82acccfc7009", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Stores the graph data in a file \"graph.pkl\" and retrieves the room node from the graph using its exit door ID, handling any exceptions.", + "description": "Within SpecificWorker, a subclass of GenericWorker, stores a graph representation of a room and its exits in a file called \"graph.pkl\".", "params": [], "returns": null, "name": "store_graph", @@ -1540,11 +1535,11 @@ "docLength": null }, { - "id": "4de8cad4-147a-8d9b-234a-1d5133da4d7d", + "id": "b4c71ece-c0c2-6f93-814a-f62b2e37eac0", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Removes nodes from the long-term graph based on their room number, preserving edges that connect rooms with the same number. It first identifies nodes with room numbers matching the input room number, then deletes nodes and their corresponding edges in the long-term graph.", + "description": "Removes edges from the graph that connect nodes representing rooms, based on their room numbers. It first identifies edges with room numbers matching those of the current room, then deletes them and updates a dictionary for future reference. Finally, it draws the updated graph and sets the state to \"idle\".", "params": [], "returns": null, "name": "removing", @@ -1560,16 +1555,16 @@ "docLength": null }, { - "id": "f0040ef8-9762-97a1-634e-f2fbeeea9223", + "id": "8162c2ce-cb2a-1689-a149-3b886804fd98", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Traverses the graph represented by an igraph object `g` and performs some operations on it. Specifically, it: (1) retrieves a node with the given `node_id`, (2) extracts its RT children edges from the graph, (3) inserts a vertex representing the current node in the graph, and (4) recursively traverses the RT children of each of these nodes.", + "description": "Traverses a graph, starting from a given node, and performs a specific operation (in this case, inserting vertices and edges) on the nodes and edges it encounters.", "params": [ { "name": "node_id", "type_name": "int", - "description": "A node ID to traverse in the graph." + "description": "Used to identify the node that the function operates on." } ], "returns": null, @@ -1586,16 +1581,16 @@ "docLength": null }, { - "id": "79a1657a-5231-9baa-8144-c7d2b2a58dee", + "id": "de268dea-32ae-40ac-af42-325662be034d", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "In the `SpecificWorker` class recursively traverses the graph, inserting vertices and edges into a DSR graph for each vertex that has a successor with a higher level than itself, and then recursively traversing the successor vertex.", + "description": "Traverses the graph, starting from a given node, and inserts vertices and edges into a DSR graph based on certain conditions.", "params": [ { "name": "node", "type_name": "igraphVertex", - "description": "Used to represent a vertex in an igraph graph." + "description": "Represented by an index in the graph." } ], "returns": null, @@ -1612,16 +1607,16 @@ "docLength": null }, { - "id": "4b40456c-9fcf-758a-2a42-5d328535710a", + "id": "e91753f5-7b0b-869b-e440-6401584da274", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Adds a new vertex to an igraph object, setting its name and ID, as well as assigning values to certain attributes. It also attempts to find an edge connecting the new vertex to a predefined \"other side door\" vertex, based on attribute values.", + "description": "Adds a vertex to an igraph graph and populates its attributes based on the given node's attributes. It also attempts to find an edge connecting the new vertex to another vertex with a matching \"other_side_door_name\" attribute, and adds that edge if found.", "params": [ { "name": "node", "type_name": "igraphNode", - "description": "Passed the vertex to be added to the graph." + "description": "Passed in to add the vertex to the graph." } ], "returns": null, @@ -1638,21 +1633,21 @@ "docLength": null }, { - "id": "1ff5dd0f-33d6-0bb1-1947-12b885b5c298", + "id": "d8e8572a-c8bd-9f80-3041-8a09b43308a2", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Inserts a new node into a graph, updating the parent node's attributes and setting the new node's attributes based on the input node.", + "description": "Takes in a parent node name and a node object, creates a new node with the appropriate attributes, and inserts it into the graph using the `GenericWorker` class's `insert_node` method.", "params": [ { "name": "parent_name", "type_name": "str", - "description": "Used to specify the name of the parent node to which the new node will be added." + "description": "Used to identify the parent node for the new vertex being inserted." }, { "name": "node", - "type_name": "dict", - "description": "Passed as an attribute of a vertex." + "type_name": "Node", + "description": "Represented as a Python dictionary containing the node's attributes and values, such as agent ID, type, and name." } ], "returns": null, @@ -1669,54 +1664,54 @@ "docLength": null }, { - "id": "c43f5780-c5b5-1db6-8f43-382b331bd896", + "id": "b9e4cd38-45f4-25b0-1543-8069bb5dc584", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Adds an edge to an igraph object based on the attributes of the given edge, including the translation and rotation of the edge.", + "description": "Modifies an existing graph by adding a new edge between two nodes based on edge attributes.", "params": [ { "name": "edge", "type_name": "igraphEdge", - "description": "Passed as an instance of Igraph Edge class, which contains attributes including origin, destination, rt translation, and rotation." + "description": "Passed as an instance of the Edge class, representing an edge in the graph with specified attributes." } ], "returns": null, "name": "insert_igraph_edge", "location": { "start": 567, - "insert": 570, + "insert": 568, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 7, + "length": 9, "docLength": null }, { - "id": "43900a48-129f-0ea2-a944-43b63dbaa2ec", + "id": "658729f3-ea08-1cab-dd46-c1207d76fc8d", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Adds an edge to the graph, connecting a node representing the origin to a node representing the destination, with the given RT values and rotation.", + "description": "Modifies an edge in a graph based on input arguments 'org' and 'dest'. It creates a new edge with the appropriate RT value and rotation, and inserts it into the graph.", "params": [ { "name": "org", - "type_name": "instance", - "description": "Representing the start node of the edge to be inserted." + "type_name": "Agent", + "description": "Used to represent the starting node of the edge." }, { "name": "dest", "type_name": "Agent", - "description": "Passed as an argument to the function, representing the destination node for which the edge is being inserted." + "description": "Used to represent the destination node of the edge being inserted." } ], "returns": null, "name": "insert_dsr_edge", "location": { - "start": 579, - "insert": 582, + "start": 581, + "insert": 584, "offset": " ", "indent": 8, "comment": null @@ -1726,17 +1721,17 @@ "docLength": null }, { - "id": "d6f2e148-3e44-84a9-bf43-dcb827871106", + "id": "68515eb1-870c-66a0-e74d-fc58b5dc6b6c", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Draws the graph using Matplotlib, clearing the axis before laying out the vertices and edges. It then plots each edge with an arrow pointing to its tail node, and displays node names at their centers. Finally, it sets limits on the x and y axes.", + "description": "Draws the graph based on the layout \"kamada_kawai\", plots edges, and displays node names using annotations.", "params": [], "returns": null, "name": "draw_graph", "location": { - "start": 609, - "insert": 610, + "start": 611, + "insert": 612, "offset": " ", "indent": 8, "comment": null @@ -1746,16 +1741,16 @@ "docLength": null }, { - "id": "f36cee33-dca3-99ac-1243-f942df6664c6", + "id": "ffe7cb5a-2c7c-f8ab-fc4a-6a26dd7fd44b", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Retrieves the room ID associated with a given node ID by accessing the `room_id` attribute of the node's attributes, and returns the room ID if successful, or -1 otherwise.", + "description": "Within SpecificWorker, a subclass of GenericWorker, retrieves the room ID associated with a given node ID by querying the attribute \"room_id\" and returning its value upon success or -1 on error.", "params": [ { "name": "node_id", "type_name": "str", - "description": "Used to retrieve a specific node from the graph." + "description": "Passed as an argument to the function, representing the unique identifier of a Node in the graph." } ], "returns": { @@ -1764,8 +1759,8 @@ }, "name": "check_element_room_number", "location": { - "start": 634, - "insert": 635, + "start": 636, + "insert": 637, "offset": " ", "indent": 8, "comment": null @@ -1775,26 +1770,26 @@ "docLength": null }, { - "id": "03d00d97-d654-dea5-514a-bd8142708d10", + "id": "2288e20d-9011-75bc-464b-b4c8b38be6ad", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Within the SpecificWorker class retrieves an element level attribute from a given node and returns its value. If the attribute is not found, it prints an error message and returns -1.", + "description": "Within the `SpecificWorker` class, checks the \"level\" attribute of a specified node in the graph and returns its value. If the attribute is not found, it prints an error message and returns -1.", "params": [ { "name": "node_id", "type_name": "str", - "description": "Used as an identifier of a node within the graph." + "description": "Used to identify a node in the graph." } ], "returns": { - "type_name": "integerints", - "description": "The level of an element with the given node ID." + "type_name": "int", + "description": "Element level of the node with the given ID" }, "name": "check_element_level", "location": { - "start": 643, - "insert": 644, + "start": 645, + "insert": 646, "offset": " ", "indent": 8, "comment": null @@ -1804,23 +1799,23 @@ "docLength": null }, { - "id": "0ea5bdac-20c3-a691-e24f-7e61ffbee8e3", + "id": "e14126be-ce12-ec84-1e4f-5b15b353df35", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Retrieves the attributes of nodes connected by RT edges in a graph, and then prints their origin and destination nodes, as well as any translation attribute found.", + "description": "Within the SpecificWorker class retrieves the room ID of a given node, then checks if there are any RT edges connecting to that node. If such an edge is found, it extracts the translation attribute and prints it. Finally, it draws the room polygon and doors using OpenCV.", "params": [ { "name": "room_node_id", "type_name": "str", - "description": "Used to retrieve the node ID of the room to be drawn." + "description": "Used to identify the node representing the room for which the picture is being generated." } ], "returns": null, "name": "generate_room_picture", "location": { - "start": 686, - "insert": 688, + "start": 688, + "insert": 690, "offset": " ", "indent": 8, "comment": null @@ -1830,23 +1825,23 @@ "docLength": null }, { - "id": "31965a19-6976-7285-2f4b-afb9ec073b71", + "id": "9ac71498-065b-518a-7348-6e9a0a780cff", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Inserts or assigns an edge with specified properties to a graph, specifically the current edge representing the worker's location.", + "description": "Inserts or assigns an edge with specified attributes to the graph represented by the `self.g` attribute, using the `insert_or_assign_edge` method provided by the Graph class.", "params": [ { "name": "room_id", "type_name": "str", - "description": "Used to specify the current room id for which the edge is being created or updated." + "description": "Used to identify the current room of the agent that is performing the action." } ], "returns": null, "name": "insert_current_edge", "location": { - "start": 721, - "insert": 723, + "start": 723, + "insert": 725, "offset": " ", "indent": 8, "comment": null @@ -1856,64 +1851,64 @@ "docLength": null }, { - "id": "f87a4863-6096-8a94-4044-9bfcb8540c8a", + "id": "19662af0-764b-af81-c34a-5d2119ef52c6", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Updates a node's type, checking if it's a door and inserting its corresponding edge in the long-term graph. If the id is the affordance node's active id, it checks if the node is completed and not active, and transitions to the crossed state.", + "description": "Updates a node's properties based on its type and other considerations, such as checking if a door node has a pre-transition edge, inserting a new vertex or edge in the graph if necessary, and drawing the graph.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to represent the unique identifier of a node in the graph." + "description": "Used to identify the node to be updated." }, { "name": "type", "type_name": "str", - "description": "Used to identify the node type, which determines the actions performed on the node when the function is called." + "description": "Used to determine the action taken on the node based on its name." } ], "returns": null, "name": "update_node", "location": { - "start": 735, - "insert": 736, + "start": 737, + "insert": 738, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 39, + "length": 41, "docLength": null }, { - "id": "aca8a629-4ef8-e5ba-9748-4191fd060065", + "id": "dadeb4c6-dedc-1490-c447-209f1bc1d73a", "ancestors": [ - "8e2d2981-1343-51a0-0a43-35e06cb1288a" + "d14b1fa8-9d9a-61a7-184c-3f4e98189154" ], - "description": "Updates an edge based on its ID, type and node IDs. It checks if the specified edge exists and is not the current edge, and sets it as the current edge if conditions are met.", + "description": "Updates the current edge of a room node based on certain conditions. If the target node is the robot id, and there are no edges of type \"current\" in the graph, the function inserts the current edge and sets it as current.", "params": [ { "name": "fr", "type_name": "int", - "description": "Representative of the ID of the node it originates from." + "description": "Referred to as \"from room\" in the code snippet provided." }, { "name": "to", "type_name": "int", - "description": "The ID of the destination node in the graph." + "description": "A reference to an integer representing a node ID in the graph." }, { "name": "type", "type_name": "str", - "description": "Set to \"RT\"." + "description": "Used to indicate the type of edge being updated, specifically \"RT\"." } ], "returns": null, "name": "update_edge", "location": { - "start": 786, - "insert": 790, + "start": 792, + "insert": 796, "offset": " ", "indent": 8, "comment": null diff --git a/.komment/komment.json b/.komment/komment.json index 17d8fad2..7052d64c 100644 --- a/.komment/komment.json +++ b/.komment/komment.json @@ -1,14 +1,15 @@ { "meta": { "version": "1", - "updated_at": "2024-07-11T09:15:57.400Z", + "updated_at": "2024-07-12T12:07:42.963Z", "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" + "d6a72e5c-096b-4179-ac39-3343ea5ec3a5", + "395b32ba-01b1-45bb-84e0-96a1b0abb44e" ] }, "lookup": [ diff --git a/agents/g2o_agent/src/specificworker.py b/agents/g2o_agent/src/specificworker.py index 5351be44..c10e653c 100644 --- a/agents/g2o_agent/src/specificworker.py +++ b/agents/g2o_agent/src/specificworker.py @@ -49,133 +49,130 @@ class SpecificWorker(GenericWorker): """ - Performs specific tasks related to a worker robot's navigation and localization - in an unknown environment, such as updating the graph, getting displacement, - computing covariance matrix, visualizing G2O real-time, and more. + Manages a graph, updates node attributes, and sets edges based on robot odometry + data and room changes. It also handles RT edge tracking and stores translation + and rotation values for later use. Attributes: - Period (float): Used to control the time interval between consecutive calls - to the `update_worker` method, which is responsible for updating the - robot's state based on the received data. The value of `Period` - determines how often the worker will be called, with larger values - resulting in slower updates but potentially reducing the load on the - system. - agent_id (int): Used as a unique identifier for the agent's id. - g (undirected): Used to store the graph representing the environment. It - contains nodes and edges that represent the obstacles, walls, and other - features of the environment. - startup_check (QTimersingleShot): Used to check for the application's quit - after a certain time interval, which is 200 milliseconds in this case. - rt_api (str): 100% sure to be a valid ROS topic name for receiving real-time - data from a robotic arm. - inner_api (instance): Used to store a reference to the inner API of the - worker, which allows for direct communication with the worker's inner - workings. - odometry_node_id (int): Used to identify the node in the graph that - corresponds to the robot's odometry information. It is used to store - the odometry data in the graph and for visualization purposes. - odometry_queue (3element): Used to store the odometry information of the - robot in a First-In-First-Out (FIFO) queue. It stores the advance, - lateral, and angular displacement of the robot at each time step, which - are computed using the G2O optimization algorithm. - last_odometry (3element): Used to store the last known odometry information - of the robot, which is used in the computation of the displacement and - covariance matrix. - g2o (Optimizer): Used to store the Gauss-Newton optimizer for graph matching. - It is used to compute marginals - and to update the position of vertices in the graph. - visualizer (Visualizer): Used to visualize the graph and edges in real-time - during the RT algorithm execution. It provides a function to update - the visualization and can be used to display the graph and edges in - different formats, such as 3D or 2D. - odometry_noise_std_dev (float): 0.1 by default, representing the standard - deviation of noise in the odometry measurements. It helps to control - the level of random fluctuations in the robot's motion. - odometry_noise_angle_std_dev (float): 0.8 by default, representing the - standard deviation of the angle noise added to the odometry measurements - for more accurate pose estimation. - measurement_noise_std_dev (float): Used to represent the standard deviation - of the noise present in the robot's measurements. It affects how much - the robot's estimate deviates from the true position. - last_room_id (int): Used to store the room ID of the last room that was - processed by the worker before its initialization was changed. - actual_room_id (int): Used to keep track of the current room ID that the - worker is in during its navigation. It is updated whenever the worker - moves from one room to another. - elapsed (float): Used to keep track of the time elapsed since the last - successful message received from the robot. It is updated every time - a message is processed, and its value represents the time spent - processing the message. - room_initialized (bool): Used to keep track of whether the worker has - initialized the room or not. It is set to False when the worker first - enters a new room, and to True when it exits that room. - iterations (int): 0-indexed, indicating the number of iterations (or passes) - that the worker has performed on the graph. It is updated each time - the worker processes a new edge or node in the graph. - hide (str): Used to indicate whether the worker should be hidden or not. - When set to "True", the worker will be hidden from the main window; - otherwise, it will be displayed. - init_graph (Python): Used to indicate whether the graph has been initialized - or not. It is set to `True` when the graph is first constructed and - then reset to `False` when the graph is updated. - current_edge_set (bool): Used to keep track of whether the current edge - set has been updated with a new RT translation or rotation. It is set - to True when an edge set is updated with a new RT translation or - rotation, and False otherwise. - first_rt_set (Python): Initialized to `True` when the agent first sets a - translation and rotation for the RT task, indicating that it is the - first time this has happened in the simulation. - translation_to_set (3D): Used to store the translation of the RT object - to the set of corners of the room. - rotation_to_set (3D): Used to store the rotation of a robot relative to + Period (instance): Used to control the update rate of the graph. It sets + the time interval between successive updates in milliseconds, which + can be used to optimize the performance of the worker. + agent_id (int): Used to identify the agent that owns the robot. It is used + to determine which agent's graph should be updated when a new edge is + added or an existing edge is modified. + g (Graph): Used for representing the robot's environment through a graph, + with nodes representing rooms and edges representing the movement of + the robot between them. + startup_check (QTimersingleShot): Used to check if the user wants to quit + the application after a certain period of time has passed since the + last update. It schedules a single shot event every 200 milliseconds + to check if the user wants to quit. + rt_api (instance): Used to store information related to the RT (Real-time) + module, such as the last time it was accessed and the translation and + rotation values set for the current room. + inner_api (instance): A method that updates the graph with new nodes, + edges, or attributes based on the inner loop of the worker. + odometry_node_id (int): Used to store the ID of the node representing the + robot's position in the graph. + odometry_queue (list): Used to store the current odometry data of the + robot, including its advance speed, side speed, and angular speed, + which are updated at a rate of 200 Hz. + last_odometry (int): Used to store the time at which the worker last + received odometry data from the environment. It is updated every time + a new odometry message is received, allowing the worker to determine + how long ago it received the message. + g2o (Graph): Used to store the graph data of the environment. It contains + the nodes, edges, and attributes of the graph. + odometry_noise_std_dev (floatingpoint): Used to specify the standard + deviation of noise added to the odometry measurements for training purposes. + odometry_noise_angle_std_dev (float): 0.1 by default, representing the + standard deviation of the noise added to the robot's angle readings + during odometry estimation. + measurement_noise_std_dev (float): 0.1 by default, representing the standard + deviation of measurement noise for the robot's sensors. It determines + how much noise is added to the measured positions and orientations of + the robot in the simulation. + last_room_id (int): Used to store the last room ID seen by the worker + before changing rooms. + actual_room_id (int): Used to store the current room ID of the agent during + navigation. + elapsed (int): Used to keep track of the time elapsed since the last call + to the `startup_check()` function, which is used to check if the + application should be closed after a certain period of inactivity. + room_initialized (bool): Used to track whether the current room has been + initialized or not, it's set to False when a new room is entered and + True otherwise. + iterations (int): 0-indexed, indicating the number of iterations of the + worker's tasks it can perform before finishing. + hide (str): Used to determine whether a node or edge should be hidden from + the graph. It allows you to specify which nodes or edges to hide based + on their ID, label, or other attributes. + init_graph (instance): Used to keep track of whether the graph has been + initialized or not, it's set to `True` when the graph is first created + and `False` otherwise. + current_edge_set (bool): Used to track whether the current edge being + processed is part of the RT set or not. + first_rt_set (bool): Set to True when a new RT (remote transmission) set + is encountered for the first time during the simulation, indicating + that the worker has entered a new room or area. + translation_to_set (3D): Used to store the translation of a robot's end + effector when it enters a specific set. It is used in conjunction with + the + `rotation_to_set` attribute to determine the full pose of the robot + when it enters the set. + rotation_to_set (3D): Used to store the rotation of the robot relative to its set position. - room_polygon (3D): Used to represent the room where the robot is currently - located. It stores a list of 3D vertices that define the shape of the - room. - security_polygon (list): Used to store a polygon representing the security - area around the worker's target location. It is used to check if the - worker's path intersects with any obstacles or security areas, ensuring - the worker's safety during its motion. - initialize_g2o_graph (instance): Responsible for initializing a Graph2O - graph object to represent the environment, which is used for computing - the robot's pose and motion. - rt_set_last_time (int): Used to keep track of the time since the last RT - set was received. It is used to determine when to send a new RT set. - rt_time_min (float): Used to set a minimum time interval between RT sets. - It serves as a threshold for determining when a new RT set should be - initiated based on the agent's movement. - last_update_with_corners (int): Used to store the last time corners were - updated. It's used in conjunction with other attributes to maintain a - consistent update schedule for corners. + room_polygon (QPolygon): Used to store the polygon representation of a + room in the environment. + security_polygon (QPolygon): Used to store the security polygon of a + specific worker in a graph, which is used for collision detection and + response in robotics. + initialize_g2o_graph (instance): Used to create a Graph2O graph from a + robot's observation, which is then used for optimization. It initializes + the graph by adding nodes and edges based on the robot's observation + and sets up the necessary attributes for optimization. + rt_set_last_time (int): Used to track the time since the last RT set was + received for a given agent ID. It is used in the `update_edge()` + function to determine if enough time has passed since the last RT set + for the robot to consider setting a new translation and rotation. + rt_time_min (float): Set to the minimum time interval between two RT sets + that are considered as a new RT set. It is used to determine when to + reset the translation and rotation to set values in the `update_edge()` + method. + last_update_with_corners (int): Used to keep track of when the worker last + updated its graph with corners. It is set to the current time whenever + the `update_node`, `update_edge`, or `delete_edge` methods are called, + and is used to determine when the graph has changed significantly + enough to warrant updating the robot's state. timer (QTimer): Used to schedule a call to the `QApplication.instance().quit()` - function every 200 milliseconds, indicating that the worker should - shut down. - compute (lambda): Used to compute the marginal probability of the worker's - variables given the observed data. It takes an index `i` as input and - returns a tuple containing the probability of each variable in the - worker's graph given the observed data, as well as the gradient of the - probability with respect to the variable's value. - update_node_att (QMetaObjectAttribute): Used to update the attributes of - a node in the graph when a new attribute is received from the ROS bag. - update_edge (str): Used to update the edge attributes based on the edge - type. It takes three parameters: `fr`, `to`, and `type`, which are the - index of the current edge, the index of the next edge, and the edge - type, respectively. The method updates the edge attributes based on - the edge type and sets the `current_edge_set` attribute to `True`. - update_edge_att (edge): Used to update the attributes of an edge in the graph. + function after 200 milliseconds, which means that the worker will quit + after a certain amount of time has passed. + compute (instance): Used to handle updates for node attributes. It takes + two arguments: `id` which is the id of the node to be updated, and + `attribute_names` which is a list of strings representing the names + of the attributes to be updated. + update_node_att (event): Called when a new node attribution is received + from the graph. It updates the robot's odometry queue with the current + advance, side, and angular speed and sets the `room_initialized` flag + to false if the room ID changes. + update_edge (update_edge): Used to update the attributes of an edge in a + graph based on its type, such as "current" or "RT". It sets the + translation and rotation of the robot based on the RT set. + update_edge_att (str): Defined as a method that updates the attributes of + an edge in the graph based on a specific edge type and a list of + attribute names. """ def __init__(self, proxy_map, startup_check=False): """ - Initializes an instance of the SpecificWorker class by setting up various - components such as g2o graph, visualizer, and timers. It also performs - startup checks and connects signals for node and edge updates. + Initializes a SpecificWorker object, setting its properties and connecting + to signals for updating node attributes, edges, and edge attributes. Args: - proxy_map (dict): Used to store mapping information between the original - graph and the transformed graph for DSR algorithm implementation. - startup_check (Optionalbool): Used to check for any errors in the graph - during initialization. + proxy_map (dict): Used to specify a mapping from real-world coordinates + to virtual coordinates for the robot's movement. + startup_check (bool): Used to run an initialization check on the graph + when the agent starts up. """ super(SpecificWorker, self).__init__(proxy_map) @@ -196,7 +193,7 @@ def __init__(self, proxy_map, startup_check=False): self.last_odometry = None # Initialize g2o graph with visualizer self.g2o = G2OGraph(verbose=False) - self.visualizer = G2OVisualizer("G2O Graph") + # self.visualizer = G2OVisualizer("G2O Graph") self.odometry_noise_std_dev = 1 # Standard deviation for odometry noise self.odometry_noise_angle_std_dev = 1 # Standard deviation for odometry noise @@ -252,9 +249,9 @@ def setParams(self, params): @QtCore.Slot() def compute(self): """ - Performs RT odometry computation based on sensor data and updates the - Graph2O graph with landmarks, affordances, and RT edges. It also maintains - counters for valid corners and doors, and rotates the robot if necessary. + In the `SpecificWorker` class implements a robot's movement using ROS + navigation stack, computing the robot's position and orientation based on + the previous odometry data and adding it to the graph. """ if time.time() - self.elapsed > 1: @@ -341,22 +338,22 @@ def compute(self): # 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) - - 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 == 4 and len(affordance_nodes) == 0: - if self.rt_set_last_time - self.last_update_with_corners > 3: - print("No affordance nodes active. Rotating robot") - opt_orientation += np.pi/8 - 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 + # self.visualizer.update_graph(self.g2o) + + # 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) @@ -383,14 +380,13 @@ def initialize_g2o_graph(self): # print("Initializing g2o graph") # get robot pose in room """ - Initializes a Graph2O graph for a specific robot and room by: - * Extracting landmarks from the robot's point cloud - * Calculating nominal corners based on landmarks and room geometry - * Adding fixed poses to the g2o graph for the robot and doors in the room. + Initializes a Graph2Online (G2O) graph for a specific robot's odometry + data, by adding nominal corners and fixed poses to the graph based on room + and door nodes in the environment map. Returns: - bool: 1 if the initialization of the g2o graph was successful, and 0 - otherwise. + bool: 1 if the function was able to initialize the g2o graph successfully, + and 0 otherwise. """ self.g2o.clear_graph() @@ -597,17 +593,17 @@ def initialize_g2o_graph(self): def get_displacement(self, odometry): """ - Computes and updates the displacement values (lateral, forward, and angular) - of a robot based on its odometry data, using a moving average of recent - positions to smooth out the motion. + Calculates the displacement of an agent based on its odometry data, taking + into account advancement, lateral movement, and angular movement. Args: - odometry (3element): An instance of the `Odometry` class, representing - the robot's position and velocity over time. + odometry (dict): Passed as an argument to the function, containing the + odometric data of the vehicle at each time step, including the + position, velocity, and timestamp. Returns: - 3element: The lateral displacement (in meters), the forward displacement - (in meters) and angular displacement (in radians). + 3element: 3-D vector representing the displacement of a robot's end + effector in terms of lateral, forward, and angular displacements. """ desplazamiento_avance = 0 @@ -630,15 +626,16 @@ def get_displacement(self, odometry): def get_covariance_matrix(self, vertex): """ - Computes the covariance matrix for a given vertex in a graph, using an - optimization algorithm to compute the marginals of the vertices and then - constructing the covariance matrix. + Computes the covariance matrix of a set of vertices in a graph, using the + gradient descent optimizer from the `g2o` library. It returns the computed + covariance matrix and whether it was successfully computed or not. Args: - vertex (g2oVertex): Used to represent a vertex in the graph. + vertex (g2overtex): Used to represent a specific vertex in a graph. Returns: - 2tuple: A pair of a boolean result and a covariance matrix. + tuple: 2-element tuple containing two values: (`covariances_result`, + `covariances`). """ cov_vertices = [(vertex.hessian_index(), vertex.hessian_index())] @@ -655,13 +652,13 @@ def get_covariance_matrix(self, vertex): def visualize_g2o_realtime(self, optimizer): """ - Within the `SpecificWorker` class loads a G2O file, visualizes the vertices - and edges of the graph in a 3D scatter plot, and updates the plot in - real-time as new measurements are received. + Visualizes the 3D positions of vertices and edges in a G2O file in real-time, + using matplotlib. It loads the G2O file, calculates the positions of the + vertices and edges, and plots them on a 3D scatter plot. Args: - optimizer (instance): An object that loads G2O files and provides - access to their vertices and edges for visualization. + optimizer (instance): Used to store an instance of the G2O optimizer + class, which is responsible for minimizing the energy of the system. """ plt.ion() @@ -710,14 +707,14 @@ def update_node_att(self, id: int, attribute_names: [str]): # print("INIT GRAPH") """ - Updates an attribute of a node in a graph, specifically the odometry node, - by appending its current position and velocity to a queue for processing. + Updates the attributes of a specific node in a graph, based on the current + time and other factors. Args: - id (int): Passed as an argument to the function, representing the - unique identifier for the node being updated. - attribute_names ([str]): A list of names of attributes to update on - the node associated with the given ID. + id (int): Used to represent the unique identifier of the node being + updated, specifically the odometry node ID. + attribute_names ([str]): An array of names of attributes to be updated + on the node. """ if id == self.odometry_node_id: @@ -738,22 +735,21 @@ def update_node(self, id: int, type: str): # self.room_initialized = True if self.initialize_g2o_graph() else False # self.init_graph = True """ - Updates a node's type, but only if the type is "corner". If it is, the - function also checks if the "room" node has been initialized and sets the - `init_graph` attribute to `True`. + Updates an unspecified node with an ID and a type. If the type is "corner", + it initializes a room graph if necessary. Otherwise, it does nothing. Args: - id (int): Used to represent the unique identifier of a node. - type (str): Used to indicate the type of node being updated, specifically - whether it is a corner node or not. + id (int): Used to represent the unique identifier for the node being + updated. + type (str): Defined as "corner". """ pass def delete_node(self, id: int): """ - Deletes a node from a list maintained by a `SpecificWorker` instance, - setting a flag to indicate that the list has been modified. + Deletes a node from a data structure managed by a `SpecificWorker` subclass + of `GenericWorker`. Args: id (int): Used to identify the node to be deleted. @@ -769,17 +765,15 @@ def delete_node(self, id: int): def update_edge(self, fr: int, to: int, type: str): """ - Updates the current room ID and RT translation and rotation when the agent - moves from one room to another, and sets the `current_edge_set` variable - to `True`. + Updates the room ID and sets the `current_edge_set` attribute based on the + type of edge received, and also performs RT translation and rotation if necessary. Args: - fr (int): A reference to a node in the graph represented by the class - instance. - to (int): Used as an index to access the target node of an edge in the - graph. - type (str): Used to identify the edge type, which can be either "current" - or "RT". + fr (int): Representative of the starting vertex of an edge in a graph. + to (int): Used to represent the ID of the next node in the graph that + the edge will be attached to. + type (str): Used to indicate the type of edge being updated, either + "current" or "RT". """ if type == "current" and self.g.get_node(fr).type == "room": @@ -811,13 +805,13 @@ def update_edge_att(self, fr: int, to: int, type: str, attribute_names: [str]): def delete_edge(self, fr: int, to: int, type: str): """ - Deletes an edge from a graph based on its FID (fr), TID (to), and edge - type (type). + Deletes an edge from a graph, specified by its ID and type. Args: - fr (int): Used as the index of the edge to be deleted. - to (int): Used to represent the destination node ID for edge deletion. - type (str): Used to specify the edge type that should be deleted. + fr (int): Passed as an argument to the function with the value of 123. + to (int): Used to specify the target vertex ID for edge deletion. + type (str): Used to specify the edge type to be deleted, which can be + either 'weighted' or 'unweighted'. """ pass diff --git a/agents/long_term_spatial_memory_agent/src/specificworker.py b/agents/long_term_spatial_memory_agent/src/specificworker.py index 229f0774..91560f33 100644 --- a/agents/long_term_spatial_memory_agent/src/specificworker.py +++ b/agents/long_term_spatial_memory_agent/src/specificworker.py @@ -48,92 +48,84 @@ class SpecificWorker(GenericWorker): """ - Manages a robot's interactions with its environment, including moving through - doors, creating new rooms, and updating node attributes. It also generates - images of rooms and handles long-term graph management. + Manages a graph representing a robot's movement and interactions with its + environment, performing various operations such as inserting, updating, and + deleting nodes and edges, as well as tracking the robot's state. Attributes: - Period (str): Used to set a fixed time interval for the worker to perform - its function, allowing to schedule the work in a more organized manner. - agent_id (int): Used to identify the worker as a specific agent, such as - a robot or human, within the framework. - g (igraph): A weighted graph that represents the environment of the agent. - It stores the nodes (rooms) and edges between them, which represent - the doors connecting the rooms. The graph is used to determine the - robot's movement and interactions with its environment. - update_node (str): Used to update a specific node in the graph based on - its ID. It takes one argument, which is the type of update (either - "door" or "room"), and performs different actions depending on the - update type. - update_edge (str): Used to update the edge type of a specific edge in the - graph. It takes two arguments: the first is the id of the starting - node, and the second is the id of the ending node, as well as an - optional third argument which is the new edge type to be set for that - edge. - startup_check (QTimersingleShot): Used to check for possible termination - of the worker after a specific timeout period of 200 milliseconds. - rt_api (instance): Used for accessing the RT API of the environment to get - information about the robot's position, orientation, and other relevant - data. - inner_api (internal): Used for communication between different agents in - a multi-agent environment, allowing them to exchange information and - coordinate their actions. - robot_name (str): Used as the ID of the robot in the internal graph, which - is generated based on the agent's name. - robot_id (int): Used to represent the unique identifier of the robot agent - in the environment. - last_robot_pose (3D): Used to store the last known position of the robot, - which can be used to determine the robot's motion and interaction with - its environment. - robot_exit_pose (str): 3D pose representation of the robot when it exits - a room. It is used to check if the robot has reached its destination - and to generate an image of the room. - state (str): Used to store the current state of the worker (either "idle", - "busy", or "crossed") indicating whether the worker is available or - not for new tasks. + Period (str): Used to store the current time period in which the worker + is active, allowing for more efficient handling of tasks during different + time slots. + agent_id (int): Used as the ID of the agent that owns or manipulates the + graph, which could be a robot or a human operator. + g (igraphGraph): Used for manipulating the graph in various methods like + `insert_igraph_vertex`, `insert_igraph_edge`, etc. + update_node (int): Used to update a node's attributes based on its ID. It + takes two arguments: `id` (int) and `type` (str), where `id` is the + node's ID to be updated and `type` represents the type of update (e.g., + "door", "room"). + update_edge (int): Used to update the edge of a robot moving from one node + to another with a certain type (current) when there is no current edge + and a room node exists. + startup_check (str): Used to check if the worker is in startup mode or + not. It is set to "true" when the worker starts up and "false" otherwise. + rt_api (str): Represented as a string value indicating which RT (Real-Time) + affordance to use. + inner_api (instance): A Python method that returns the inner API of the + worker. It is used to define the functionality of the worker and its + interactions with other components of the system. + robot_name (str): Used to store the name of the robot being controlled by + the worker. + robot_id (int): Used to identify the robot node in the graph. + last_robot_pose (IGraph): Used to store the last known pose of the robot, + which can be used for debugging purposes or to track the robot's movement. + robot_exit_pose (RT): A dictionary containing the pose (position and + orientation) of the robot when it exits the room through the door. + state (str): Used to store the current state of the worker, which can be + either "idle", "working", or "crossed". affordance_node_active_id (int): Used to store the ID of the affordance - node that indicates when a robot reaches a certain affordance, it will - stop moving and wait for further instructions. - exit_door_id (int): 13 by default, representing the index of the door node - that marks the exit of a room in the graph. It is used to identify the - door through which the robot exits a room when it needs to navigate - to another room. - room_exit_door_id (int): 0-indexed. It represents the ID of the door - connecting a room to the environment. It is used to identify the door - through which the robot can exit a room. - enter_room_node_id (int): Used to store the ID of the room node that the - worker enters after crossing the door. - graph (igraphGraph): Used to store the agent's internal graph, which - represents the environment and the robot's movements. - vertex_size (int): 16 by default, which means that each vertex in the graph - has a size of 16 pixels in the internal graph representation. - not_required_attrs (list): Used to store a list of attribute names that - are not required for the worker's functionality, but may be useful for + node that is currently active. It is used to check if the affordance + node is completed and not active, and to switch to the crossed state + when it is completed and not active. + exit_door_id (int): 1234, which represents the ID of the door node that + serves as the exit of a room. + room_exit_door_id (int): Used to represent the ID of the door node that + leads from a room to the outside world in the graph. + enter_room_node_id (int): 0 by default, representing the ID of the room + node that the worker enters when it completes its task. + graph (igraphGraph): Used for storing and manipulating the graph represented + by the worker. It contains the nodes, edges, and other attributes of + the graph. + vertex_size (int): Used to store the size of the vertex (node) in the + graph, indicating the number of attributes associated with each node. + not_required_attrs (list): Used to keep track of a list of attributes that + are not required for the worker's functionality, but can be useful for debugging or other purposes. long_term_graph (igraphGraph): Used for storing the long-term graph of the - environment, which is different from the short-term graph used for navigation. - global_map (igraphGraph): Used to store the global map of the environment. - It is used to represent the entire environment and its connections, - rather than just the local area around a single agent or node. - insert_current_edge (Edge): Used to insert a new edge in the graph with - the current room as the source and the robot as the destination, with - the "current" edge type. - timer (QTimer): Used to schedule the worker's update loop to run after a - certain delay (200 milliseconds in this case). - compute (instance): Used to compute the worker's output for a given input. + environment, which is updated when certain events occur like inserting + or deleting nodes. + global_map (igraphGraph): Used to store the global map of the environment, + which is a graph representation of the environment that includes all + nodes and edges. + insert_current_edge (igraphEdge): Used to insert a new edge into the graph + with the given source and target nodes, and the specified edge type. + timer (int): Set to the number of milliseconds since the epoch (January + 1, 1970, 00:00:00 UTC) when the worker was created. It is used to track + the elapsed time for the worker's tasks. + compute (instance): Used to compute the shortest path between two nodes + in the graph based on the RT model. """ def __init__(self, proxy_map, startup_check=False): """ - Initializes an object of the `SpecificWorker` class, setting its graph, - period, and other properties. + Initializes an instance of the SpecificWorker class, setting up graph + connections and variables for storing information about a robot's environment + and state. Args: - proxy_map (igraphGraph): Used to initialize the graph object for the - specific worker's long-term spatial memory. - startup_check (bool): Used to check if the graph has been modified - since the last startup. If set to True, it will call the `startup_check` - method, otherwise it will skip it. + proxy_map (igraphGraph): Used to represent the graph mapping between + nodes and objects, allowing for efficient object manipulation. + startup_check (bool): Used to check if the agent has already been started. """ super(SpecificWorker, self).__init__(proxy_map) @@ -203,12 +195,13 @@ def __del__(self): def setParams(self, params): """ - Sets parameters for an object, removes a self-edge from a room, stores ID - of the exit door, names both doors with the other side door attribute, and - reads entrance door node to add other side door attributes. + Sets the parameters of an object, then removes a self-edge from a room and + stores the ID of an exit door in a variable. It also assigns attributes + to both doors in the new room. Args: - params (object): Passed to set parameters for an object of class `Room`. + params (object): Used to store any relevant data or configuration for + the function's operation. Returns: Boolean: True. @@ -247,10 +240,9 @@ def compute(self): # print(origin_level, destination_level) """ - Determines the current state of the worker and performs the appropriate - action based on that state, including idling, crossing, crossing, initializing - rooms, knowing rooms, initializing doors, storing the graph, and removing - nodes. + Determines the current state of a worker and performs the appropriate + actions based on that state, including idle, crossing, crossed, initializing + room, known room, initializing doors, storing graph, and removing. """ match self.state: @@ -275,12 +267,11 @@ def compute(self): 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 """ - Performs affordance-based navigation for an autonomous robot, specifically: + Performs the following tasks: 1/ Identifies active affordance nodes in the short-term graph. - 2/ Checks if any "current" edge exists and if so, identifies the door node - leading to the next room. - 3/ Computes the final robot pose and door pose in the new room reference - frame using the inner API. + 2/ Checks if any "current" edge exists in the short-term graph. + 3/ If a door is found, computes its pose in the long-term graph and + associates it with the current room. """ aff_cross_nodes = [node for node in self.g.get_nodes_by_type("affordance") if node.attrs["active"].value == True] @@ -385,8 +376,9 @@ def idle(self): def crossed(self): # Get parent node of affordance node """ - Determines whether a node represents an exit door and, if so, updates the - state of the `SpecificWorker` instance to reflect this information. + Determines the current room based on an active affordance node and updates + the state machine accordingly. If the node has no parent, it sets the exit + door ID to the value of the active affordance node's parent attribute. """ affordance_node = self.g.get_node(self.affordance_node_active_id) @@ -411,8 +403,8 @@ def initializing_room(self): # Get room nodes """ - Determines and remembers the room node ID that the worker will enter after - leaving the current room, and updates the state to "initializing doors". + 1) identifies room nodes in the graph, 2) sets the `enter_room_node_id` + field, and 3) enters the "initializing doors" state. """ 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] @@ -429,9 +421,9 @@ def initializing_room(self): def known_room(self): # Get other side door name attribute """ - Within the SpecificWorker class updates the RT object's position and - rotation based on the last known affordance pose, taking into account the - robot's movement and the associated door found in the global map. + Within the `SpecificWorker` class defines how a robot navigates through a + graph representation of a maze to reach an objective room while avoiding + obstacles and taking into account the associated door's rotation. """ other_side_door_node = self.g.get_node(self.exit_door_id) @@ -529,8 +521,8 @@ def known_room(self): def initializing_doors(self): # Check if node called "room_entry" of type room exists """ - 1) finds edges leading to the exit door, 2) checks if any match edges - exist, and 3) updates node attributes and associations for doors involved. + Identifies doors connected to a specified exit door and associates them + with each other, updating node attributes and the state of the worker. """ exit_edges = [edge for edge in self.g.get_edges_by_type("exit") if edge.destination == self.exit_door_id] @@ -573,14 +565,15 @@ def initializing_doors(self): def associate_doors(self, door_1, door_2): # Find each door in igraph and update attributes """ - Adds an edge to a graph between two nodes representing doors, and sets - their "other side door name" and "connected room name" attributes based - on the input door names. + 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 name of a door node in the graph. - door_2 (str): Represented as a list of two elements, where each element - represents the name of a door node in the graph. + door_1 (str): A string representing the name of the first door to be + associated with another door in the graph. + door_2 (str): Represented as an igraph vertex object, which contains + information about the door node to be associated with the first + door node passed as input. """ try: @@ -602,8 +595,8 @@ def associate_doors(self, door_1, door_2): def store_graph(self): """ - Stores the graph data in a file "graph.pkl" and retrieves the room node - from the graph using its exit door ID, handling any exceptions. + Within SpecificWorker, a subclass of GenericWorker, stores a graph + representation of a room and its exits in a file called "graph.pkl". """ actual_room_node = self.g.get_node(self.room_exit_door_id) @@ -622,10 +615,11 @@ def store_graph(self): def removing(self): # # Get last number in the name of the room """ - Removes nodes from the long-term graph based on their room number, preserving - edges that connect rooms with the same number. It first identifies nodes - with room numbers matching the input room number, then deletes nodes and - their corresponding edges in the long-term graph. + Removes edges from the graph that connect nodes representing rooms, based + on their room numbers. It first identifies edges with room numbers matching + those of the current room, then deletes them and updates a dictionary for + future reference. Finally, it draws the updated graph and sets the state + to "idle". """ room_number = self.g.get_node(self.room_exit_door_id).attrs["room_id"].value @@ -655,14 +649,12 @@ def removing(self): def traverse_graph(self, node_id): # Mark the current node as visited and print it """ - Traverses the graph represented by an igraph object `g` and performs some - operations on it. Specifically, it: (1) retrieves a node with the given - `node_id`, (2) extracts its RT children edges from the graph, (3) inserts - a vertex representing the current node in the graph, and (4) recursively - traverses the RT children of each of these nodes. + Traverses a graph, starting from a given node, and performs a specific + operation (in this case, inserting vertices and edges) on the nodes and + edges it encounters. Args: - node_id (int): A node ID to traverse in the graph. + node_id (int): Used to identify the node that the function operates on. """ node = self.g.get_node(node_id) @@ -675,13 +667,11 @@ def traverse_graph(self, node_id): def traverse_igraph(self, node): """ - In the `SpecificWorker` class recursively traverses the graph, inserting - vertices and edges into a DSR graph for each vertex that has a successor - with a higher level than itself, and then recursively traversing the - successor vertex. + Traverses the graph, starting from a given node, and inserts vertices and + edges into a DSR graph based on certain conditions. Args: - node (igraphVertex): Used to represent a vertex in an igraph graph. + node (igraphVertex): Represented by an index in the graph. """ vertex_successors = self.graph.successors(node.index) @@ -698,13 +688,13 @@ def traverse_igraph(self, node): def insert_igraph_vertex(self, node): """ - Adds a new vertex to an igraph object, setting its name and ID, as well - as assigning values to certain attributes. It also attempts to find an - edge connecting the new vertex to a predefined "other side door" vertex, - based on attribute values. + Adds a vertex to an igraph graph and populates its attributes based on the + given node's attributes. It also attempts to find an edge connecting the + new vertex to another vertex with a matching "other_side_door_name" + attribute, and adds that edge if found. Args: - node (igraphNode): Passed the vertex to be added to the graph. + node (igraphNode): Passed in to add the vertex to the graph. """ self.graph.add_vertex(name=node.name, id=node.id, type=node.type) @@ -744,13 +734,15 @@ def insert_igraph_vertex(self, node): def insert_dsr_vertex(self, parent_name, node): # print("Inserting vertex", node["name"], node["type"]) """ - Inserts a new node into a graph, updating the parent node's attributes and - setting the new node's attributes based on the input node. + Takes in a parent node name and a node object, creates a new node with the + appropriate attributes, and inserts it into the graph using the `GenericWorker` + class's `insert_node` method. Args: - parent_name (str): Used to specify the name of the parent node to which - the new node will be added. - node (dict): Passed as an attribute of a vertex. + parent_name (str): Used to identify the parent node for the new vertex + being inserted. + node (Node): Represented as a Python dictionary containing the node's + attributes and values, such as agent ID, type, and name. """ new_node = Node(agent_id=self.agent_id, type=node["type"], name=node["name"]) @@ -766,20 +758,21 @@ 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): - - # Search for the origin and destination nodes in the graph """ - Adds an edge to an igraph object based on the attributes of the given edge, - including the translation and rotation of the edge. + Modifies an existing graph by adding a new edge between two nodes based + on edge attributes. Args: - edge (igraphEdge): Passed as an instance of Igraph Edge class, which - contains attributes including origin, destination, rt translation, - and rotation. + edge (igraphEdge): Passed as an instance of the Edge class, representing + an edge in the graph with specified attributes. """ - origin_node = self.graph.vs.find(id=edge.origin) - destination_node = self.graph.vs.find(id=edge.destination) + 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"]) @@ -791,13 +784,14 @@ def insert_dsr_edge(self, org, dest): # print("ORG::", org) # self.insert_dsr_vertex(dest) """ - Adds an edge to the graph, connecting a node representing the origin to a - node representing the destination, with the given RT values and rotation. + Modifies an edge in a graph based on input arguments 'org' and 'dest'. It + creates a new edge with the appropriate RT value and rotation, and inserts + it into the graph. Args: - org (instance): Representing the start node of the edge to be inserted. - dest (Agent): Passed as an argument to the function, representing the - destination node for which the edge is being inserted. + org (Agent): Used to represent the starting node of the edge. + dest (Agent): Used to represent the destination node of the edge being + inserted. """ if org is None: @@ -829,10 +823,8 @@ def insert_dsr_edge(self, org, dest): def draw_graph(self): """ - Draws the graph using Matplotlib, clearing the axis before laying out the - vertices and edges. It then plots each edge with an arrow pointing to its - tail node, and displays node names at their centers. Finally, it sets - limits on the x and y axes. + Draws the graph based on the layout "kamada_kawai", plots edges, and + displays node names using annotations. """ self.ax.clear() @@ -861,12 +853,13 @@ def draw_graph(self): def check_element_room_number(self, node_id): """ - Retrieves the room ID associated with a given node ID by accessing the - `room_id` attribute of the node's attributes, and returns the room ID if - successful, or -1 otherwise. + Within SpecificWorker, a subclass of GenericWorker, retrieves the room ID + associated with a given node ID by querying the attribute "room_id" and + returning its value upon success or -1 on error. Args: - node_id (str): Used to retrieve a specific node from the graph. + node_id (str): Passed as an argument to the function, representing the + unique identifier of a Node in the graph. Returns: int: The room ID associated with a given node ID. @@ -882,15 +875,15 @@ def check_element_room_number(self, node_id): def check_element_level(self, node_id): """ - Within the SpecificWorker class retrieves an element level attribute from - a given node and returns its value. If the attribute is not found, it - prints an error message and returns -1. + Within the `SpecificWorker` class, checks the "level" attribute of a + specified node in the graph and returns its value. If the attribute is not + found, it prints an error message and returns -1. Args: - node_id (str): Used as an identifier of a node within the graph. + node_id (str): Used to identify a node in the graph. Returns: - integerints: The level of an element with the given node ID. + int: Element level of the node with the given ID """ node = self.g.get_node(node_id) @@ -938,12 +931,14 @@ def check_element_level(self, node_id): def generate_room_picture(self, room_node_id): """ - Retrieves the attributes of nodes connected by RT edges in a graph, and - then prints their origin and destination nodes, as well as any translation - attribute found. + Within the SpecificWorker class retrieves the room ID of a given node, + then checks if there are any RT edges connecting to that node. If such an + edge is found, it extracts the translation attribute and prints it. Finally, + it draws the room polygon and doors using OpenCV. Args: - room_node_id (str): Used to retrieve the node ID of the room to be drawn. + room_node_id (str): Used to identify the node representing the room + for which the picture is being generated. """ room_node = self.g.get_node(room_node_id) @@ -982,12 +977,13 @@ def generate_room_picture(self, room_node_id): def insert_current_edge(self, room_id): # Insert current edge to the room """ - Inserts or assigns an edge with specified properties to a graph, specifically - the current edge representing the worker's location. + Inserts or assigns an edge with specified attributes to the graph represented + by the `self.g` attribute, using the `insert_or_assign_edge` method provided + by the Graph class. Args: - room_id (str): Used to specify the current room id for which the edge - is being created or updated. + room_id (str): Used to identify the current room of the agent that is + performing the action. """ current_edge = Edge(room_id, room_id, "current", self.agent_id) @@ -1004,15 +1000,14 @@ def update_node_att(self, id: int, attribute_names: [str]): def update_node(self, id: int, type: str): """ - Updates a node's type, checking if it's a door and inserting its corresponding - edge in the long-term graph. If the id is the affordance node's active id, - it checks if the node is completed and not active, and transitions to the - crossed state. + Updates a node's properties based on its type and other considerations, + such as checking if a door node has a pre-transition edge, inserting a new + vertex or edge in the graph if necessary, and drawing the graph. Args: - id (int): Used to represent the unique identifier of a node in the graph. - type (str): Used to identify the node type, which determines the actions - performed on the node when the function is called. + id (int): Used to identify the node to be updated. + type (str): Used to determine the action taken on the node based on + its name. """ if type == "door": @@ -1039,8 +1034,12 @@ def update_node(self, id: int, type: str): 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.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) @@ -1070,14 +1069,15 @@ def update_edge(self, fr: int, to: int, type: str): # TODO: Posible problema si tocas el nodo room en la interfaz gráfica """ - Updates an edge based on its ID, type and node IDs. It checks if the - specified edge exists and is not the current edge, and sets it as the - current edge if conditions are met. + Updates the current edge of a room node based on certain conditions. If + the target node is the robot id, and there are no edges of type "current" + in the graph, the function inserts the current edge and sets it as current. Args: - fr (int): Representative of the ID of the node it originates from. - to (int): The ID of the destination node in the graph. - type (str): Set to "RT". + fr (int): Referred to as "from room" in the code snippet provided. + to (int): A reference to an integer representing a node ID in the graph. + type (str): Used to indicate the type of edge being updated, specifically + "RT". """ 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: From 61dfdec6c583a0de659043070161407234a1ba7c Mon Sep 17 00:00:00 2001 From: "komment-ai[bot]" <122626893+komment-ai[bot]@users.noreply.github.com> Date: Mon, 15 Jul 2024 13:17:01 +0000 Subject: [PATCH 07/10] Added comments to 64 functions across 4 files --- .komment/00000.json | 1851 +++++++++++++---- .komment/komment.json | 8 +- agents/g2o_agent/src/specificworker.py | 359 ++-- .../src/long_term_graph.py | 57 +- .../src/specificworker.py | 528 ++--- .../src/specificworker_sec.py | 1100 ++++++++++ 6 files changed, 3026 insertions(+), 877 deletions(-) create mode 100644 agents/long_term_spatial_memory_agent/src/specificworker_sec.py diff --git a/.komment/00000.json b/.komment/00000.json index bb9415f6..d99aa9a7 100644 --- a/.komment/00000.json +++ b/.komment/00000.json @@ -4,42 +4,42 @@ "path": "agents/long_term_spatial_memory_agent/src/long_term_graph.py", "content": { "structured": { - "description": "A class RoomReconstructionPlotter and uses it to plot a metric map, including rooms and doors, based on a graph representation of a building. It utilizes various functions from the scipy library for geometric calculations and the matplotlib library for plotting. The code first loads a building graph from a text file, then calculates the room corners and their object coordinates in the room frame using recursive functions get_room_corners_recursive and get_room_objects_coordinates. Finally, it draws the rooms and doors on a matplotlib figure using annotate and arrow functions.", + "description": "a class `Room` that represents a 2D room with walls, doors, and objects. It uses the Graphviz library to visualize the room using the DOT language. The code has several methods for computing various aspects of the room, such as getting the center point of the room, drawing walls and doors, and computing the projective coordinates of objects in the room frame.\n\nThe `get_room_corners` method computes the corners of a room by traversing the room graph and finding all nodes with type \"corner\". It then returns a list of these nodes. The `get_room_objects_coordinates` method computes the projective coordinates of objects in the room frame by recursively traversing the room graph, finding all nodes with a given object type, and computing the rotation matrix and rotation vector for each object using the shortest path algorithm.\n\nOverall, this code provides a flexible framework for representing and manipulating 2D rooms with various features, such as walls, doors, and objects, using Graphviz for visualization.", "items": [ { - "id": "4b9ea1e4-30c5-c68b-774f-774fe4b224b6", + "id": "a5b8dc06-fd09-cd98-cb49-455fa8997129", "ancestors": [], - "description": "Draws a long-term graph representation of a building's layout and room usage, allowing for interactive exploration and analysis of the data. It provides methods to draw the graph, add points, and display room names and door connections.", + "description": "Acts as a plotter for a graph representation of a metric reconstruction. It provides methods to draw rooms and doors, and to customize the plot's appearance.", "attributes": [ { "name": "g", - "type_name": "instance", - "description": "Used to specify the color scheme for the graph, where `'r'` represents red, `'b'` represents blue, and `-` represents gray." + "type_name": "str|int", + "description": "Used to specify the color of the graph's edges, defaulting to 'r-' for non-current rooms and 'g-' for current rooms." }, { "name": "read_graph", - "type_name": "instance", - "description": "Used to read a graph from a file specified by the user." + "type_name": "Callable[[Union[str,Path],int],Graph]", + "description": "Used to read a graph from a file specified by the file path or name." }, { "name": "fig", "type_name": "matplotlibfigureFigure", - "description": "Used to store the figure object that represents the graph." + "description": "Used to represent the figure instance for visualization of the graph." }, { "name": "ax", - "type_name": "matplotlibaxesAxes", - "description": "Used to interact with a specific axis object in a figure. It provides methods for adding plots, annotations, and other visual elements to the axis." + "type_name": "matplotlibpyplotAxes", + "description": "Used to represent the axis of the graph. It provides methods for setting various properties of the axes, such as titles, labels, limits, and annotations." }, { "name": "fig_2", "type_name": "matplotlibfigureFigure", - "description": "Used to store the figure object for the metric reconstruction plot." + "description": "Used to store the second figure object that is created for visualizing the metric reconstruction." }, { "name": "ax_2", - "type_name": "matplotlibfigureFigure", - "description": "Used to represent the second plot, which shows the metric reconstruction." + "type_name": "matplotlibpyplotAxes", + "description": "Used to access and manipulate the second axes object created by the `subplot()` function in the parent plot." } ], "name": "LongTermGraph", @@ -51,23 +51,28 @@ "comment": null }, "item_type": "class", - "length": 269, + "length": 271, "docLength": null }, { - "id": "77430e1d-d7e6-50b3-8b46-4c2cab75420d", + "id": "a7b64245-15fe-bab5-fc4f-fd7f097e041e", "ancestors": [ - "4b9ea1e4-30c5-c68b-774f-774fe4b224b6" + "a5b8dc06-fd09-cd98-cb49-455fa8997129" ], - "description": "Reads a graph from a file, creates two subplots for visualization, and sets up the x-axis and y-axis labels.", + "description": "Reads a graph from a file and creates an instance of the Graph class, optionally creating a figure and axes to visualize the graph.", "params": [ { "name": "file_name", "type_name": "str", - "description": "Used to specify the path to a GraphML file from which the graph structure is read." + "description": "Used to specify the path to a graph file from which the graph will be read." } ], "returns": null, + "usage": { + "language": "python", + "code": "# Calling the initialization method of the class LongTermGraph with file_name as a parameter\nltg = LongTermGraph('file.txt')\n\n# Accessing and using the graph instance read from the file\nprint(ltg.g)\nprint(ltg.g.summary())\n", + "description": "" + }, "name": "__init__", "location": { "start": 16, @@ -76,28 +81,33 @@ "indent": 8, "comment": null }, - "item_type": "method", + "item_type": "constructor", "length": 22, "docLength": null }, { - "id": "81650769-f298-6a87-d146-f6ccf1580992", + "id": "99491a89-ca41-e68a-5e46-7cd175e3030f", "ancestors": [ - "4b9ea1e4-30c5-c68b-774f-774fe4b224b6" + "a5b8dc06-fd09-cd98-cb49-455fa8997129" ], - "description": "Generates a graph representation of a long-term state machine (LTSM) based on its adjacency matrix. It creates the graph layout, defines edge colors, and adds node and edge labels.", + "description": "Creates a graph based on a directed graph represented by a NetworkX graph object, and displays it using Matplotlib's scatter and plot functions.", "params": [ { "name": "only_rooms", "type_name": "bool", - "description": "Used to filter nodes based on their type. It restricts the subgraph to only include nodes labeled as \"room\"." + "description": "Used to filter the nodes and edges of the graph according to their types, only including rooms and doors." } ], "returns": null, + "usage": { + "language": "python", + "code": "import matplotlib.pyplot as plt\nfrom LongTermGraph import LongTermGraph\n\n# Create a graph object from a file\ngt = LongTermGraph(\"path/to/file\")\n\n# Draw the graph with only rooms and doors\ngt.draw_graph(only_rooms=True)\n\n# Update the plot\nplt.show()\n", + "description": "" + }, "name": "draw_graph", "location": { - "start": 253, - "insert": 254, + "start": 254, + "insert": 255, "offset": " ", "indent": 8, "comment": null @@ -241,187 +251,187 @@ "path": "agents/g2o_agent/src/specificworker.py", "content": { "structured": { - "description": "An object `Odometry` that handles updating a graph and nodes within it based on various events such as node creations, deletions, updates, and edges updates. It also manages a queue for odometry data and has methods to update and delete nodes and edges based on their types. The code uses the `networkx` package for working with graphs.", + "description": "A class `RosOdometry` that handles robot odometry data from a ROS node. It has various methods for updating and deleting nodes and edges in the graph, as well as handling different types of updates such as current position, RT translation, and room changes. The code uses the `graph` package to handle the graph structure and the `rospy` package for ROS-related functionality.", "items": [ { - "id": "db065633-a20c-b4a3-b546-23abf1d410fe", + "id": "81834b2b-13a8-409b-7041-ea7aad5792ec", "ancestors": [], - "description": "Manages a graph, updates node attributes, and sets edges based on robot odometry data and room changes. It also handles RT edge tracking and stores translation and rotation values for later use.", + "description": "Manages a specific robot's state and updates its graph, edges, and attributes based on messages from the occupancy grid. It also handles room changes and RT sets for the specified robot.", "attributes": [ { "name": "Period", - "type_name": "instance", - "description": "Used to control the update rate of the graph. It sets the time interval between successive updates in milliseconds, which can be used to optimize the performance of the worker." + "type_name": "float|int", + "description": "200 by default, indicating the time interval (in milliseconds) between updates of the graph. It can be modified to adjust the update rate of the worker." }, { "name": "agent_id", - "type_name": "int", - "description": "Used to identify the agent that owns the robot. It is used to determine which agent's graph should be updated when a new edge is added or an existing edge is modified." + "type_name": "int|str", + "description": "A unique identifier for the agent, which can be used to identify specific agents within the system." }, { "name": "g", - "type_name": "Graph", - "description": "Used for representing the robot's environment through a graph, with nodes representing rooms and edges representing the movement of the robot between them." + "type_name": "Graph|QGraphicsScene", + "description": "Used to store and manipulate the graph data structure." }, { "name": "startup_check", - "type_name": "QTimersingleShot", - "description": "Used to check if the user wants to quit the application after a certain period of time has passed since the last update. It schedules a single shot event every 200 milliseconds to check if the user wants to quit." + "type_name": "QTimersingleShot200,QApplicationinstancequit", + "description": "Set to call the quit function of the QApplication instance after a delay of 200 milliseconds." }, { "name": "rt_api", - "type_name": "instance", - "description": "Used to store information related to the RT (Real-time) module, such as the last time it was accessed and the translation and rotation values set for the current room." + "type_name": "Union[int,str]", + "description": "Used to store the API for the Robot Translation (RT) functionality in the worker. It can take the value of either an integer representing the RT algorithm or a string representing the RT algorithm name." }, { "name": "inner_api", - "type_name": "instance", - "description": "A method that updates the graph with new nodes, edges, or attributes based on the inner loop of the worker." + "type_name": "Dict[str,Any]", + "description": "Used to store additional internal data for the worker, such as the current room ID or the first RT set. It is only accessible within the worker's implementation and is not part of the public API." }, { "name": "odometry_node_id", "type_name": "int", - "description": "Used to store the ID of the node representing the robot's position in the graph." + "description": "Used to identify the node in the graph that represents the robot's position and orientation." }, { "name": "odometry_queue", - "type_name": "list", - "description": "Used to store the current odometry data of the robot, including its advance speed, side speed, and angular speed, which are updated at a rate of 200 Hz." + "type_name": "List[Tuple[float,float,float,int]]", + "description": "Used to store the latest odometry data from the robot's sensors for graph updating." }, { "name": "last_odometry", - "type_name": "int", - "description": "Used to store the time at which the worker last received odometry data from the environment. It is updated every time a new odometry message is received, allowing the worker to determine how long ago it received the message." + "type_name": "float|List[float]", + "description": "Used to store the last known odometry information of the robot, including its position, velocity, and angular velocity, which are updated every 200 milliseconds." }, { "name": "g2o", - "type_name": "Graph", - "description": "Used to store the graph data of the environment. It contains the nodes, edges, and attributes of the graph." + "type_name": "Graph|npndarray", + "description": "Used to represent the graph of the environment, allowing the worker to perform operations on it such as adding or deleting nodes and edges." }, { "name": "odometry_noise_std_dev", - "type_name": "floatingpoint", - "description": "Used to specify the standard deviation of noise added to the odometry measurements for training purposes." + "type_name": "float|int", + "description": "Used to represent the standard deviation of noise in the odometry measurements. It determines how much the actual robot position, orientation, and velocity are expected to deviate from their predicted values based on the odometry measurements." }, { "name": "odometry_noise_angle_std_dev", - "type_name": "float", - "description": "0.1 by default, representing the standard deviation of the noise added to the robot's angle readings during odometry estimation." + "type_name": "float|int", + "description": "1.0 by default, which represents the standard deviation of the noise angle in the odometry measurement. It helps to quantify the uncertainty in the estimated angle of the robot's movement." }, { "name": "measurement_noise_std_dev", - "type_name": "float", - "description": "0.1 by default, representing the standard deviation of measurement noise for the robot's sensors. It determines how much noise is added to the measured positions and orientations of the robot in the simulation." + "type_name": "float|int", + "description": "Used to represent the standard deviation of measurement noise in the robot's odometry readings. It is used to estimate the uncertainty of the robot's position and velocity." }, { "name": "last_room_id", - "type_name": "int", - "description": "Used to store the last room ID seen by the worker before changing rooms." + "type_name": "str|int", + "description": "Used to store the last room ID seen by the agent before changing rooms, used for updating the RT set." }, { "name": "actual_room_id", - "type_name": "int", - "description": "Used to store the current room ID of the agent during navigation." + "type_name": "str|int", + "description": "Used to store the current room ID of the agent, which is updated when the agent moves to a new room." }, { "name": "elapsed", - "type_name": "int", - "description": "Used to keep track of the time elapsed since the last call to the `startup_check()` function, which is used to check if the application should be closed after a certain period of inactivity." + "type_name": "float|int", + "description": "Used to keep track of the time since the last call to the `update()` method, which is used to control the frequency of updates to the graph." }, { "name": "room_initialized", "type_name": "bool", - "description": "Used to track whether the current room has been initialized or not, it's set to False when a new room is entered and True otherwise." + "description": "Set to False when a room change is detected, indicating that the worker has switched rooms. It is then reset to True once the new room's ID is determined." }, { "name": "iterations", - "type_name": "int", - "description": "0-indexed, indicating the number of iterations of the worker's tasks it can perform before finishing." + "type_name": "int|List[int]", + "description": "Used to keep track of the number of iterations of the worker's function that have been performed, or the list of iteration numbers if the function is called multiple times with different inputs." }, { "name": "hide", - "type_name": "str", - "description": "Used to determine whether a node or edge should be hidden from the graph. It allows you to specify which nodes or edges to hide based on their ID, label, or other attributes." + "type_name": "bool", + "description": "Used to hide the worker from the graphical user interface (GUI) when set to True." }, { "name": "init_graph", - "type_name": "instance", - "description": "Used to keep track of whether the graph has been initialized or not, it's set to `True` when the graph is first created and `False` otherwise." + "type_name": "bool", + "description": "Used to track whether the graph has been initialized by the worker during its lifetime. It is used to prevent unnecessary work from being performed when the graph has already been initialized." }, { "name": "current_edge_set", "type_name": "bool", - "description": "Used to track whether the current edge being processed is part of the RT set or not." + "description": "Used to indicate whether the current edge being processed is part of a RT set or not. It is set to True when a new RT edge is encountered, and False otherwise." }, { "name": "first_rt_set", "type_name": "bool", - "description": "Set to True when a new RT (remote transmission) set is encountered for the first time during the simulation, indicating that the worker has entered a new room or area." + "description": "Set to True when the robot performs its first RT (Real-time) movement, indicating that the worker has started tracking RT movements." }, { "name": "translation_to_set", - "type_name": "3D", - "description": "Used to store the translation of a robot's end effector when it enters a specific set. It is used in conjunction with the \n`rotation_to_set` attribute to determine the full pose of the robot when it enters the set." + "type_name": "float|List[float]", + "description": "Used to store the translation value to set for the shadow robot. The list stores the values of the translation in the x, y, and z dimensions respectively." }, { "name": "rotation_to_set", - "type_name": "3D", - "description": "Used to store the rotation of the robot relative to its set position." + "type_name": "float|int", + "description": "Used to store the rotation of the robot to set its position relative to its starting point." }, { "name": "room_polygon", - "type_name": "QPolygon", - "description": "Used to store the polygon representation of a room in the environment." + "type_name": "Tuple[float,float,float,]", + "description": "Used to store the coordinates of a room's polygon in a graph." }, { "name": "security_polygon", - "type_name": "QPolygon", - "description": "Used to store the security polygon of a specific worker in a graph, which is used for collision detection and response in robotics." + "type_name": "Polygon|List[Point]", + "description": "Used to store the security polygon of a room, which is used for collision detection and avoidance during robot navigation." }, { "name": "initialize_g2o_graph", - "type_name": "instance", - "description": "Used to create a Graph2O graph from a robot's observation, which is then used for optimization. It initializes the graph by adding nodes and edges based on the robot's observation and sets up the necessary attributes for optimization." + "type_name": "void|QTimersingleShot200,QApplicationinstancequit", + "description": "Used to initialize a Graph2O graph for representing the environment." }, { "name": "rt_set_last_time", - "type_name": "int", - "description": "Used to track the time since the last RT set was received for a given agent ID. It is used in the `update_edge()` function to determine if enough time has passed since the last RT set for the robot to consider setting a new translation and rotation." + "type_name": "int|float", + "description": "Used to track the time since the last RT (Room To) set by the agent. It is used to determine when to reset the translation and rotation to set values." }, { "name": "rt_time_min", - "type_name": "float", - "description": "Set to the minimum time interval between two RT sets that are considered as a new RT set. It is used to determine when to reset the translation and rotation to set values in the `update_edge()` method." + "type_name": "float|int", + "description": "Defined as a minimum time gap between two successive RT edge sets, used to determine when to update the translation and rotation values for the agent." }, { "name": "last_update_with_corners", - "type_name": "int", - "description": "Used to keep track of when the worker last updated its graph with corners. It is set to the current time whenever the `update_node`, `update_edge`, or `delete_edge` methods are called, and is used to determine when the graph has changed significantly enough to warrant updating the robot's state." + "type_name": "int|bool", + "description": "Used to keep track of when the worker has last updated its state with corners data. It is initially set to True, indicating that the worker has not yet received any corners data, and then is updated to False whenever the worker receives new corners data." }, { "name": "timer", - "type_name": "QTimer", - "description": "Used to schedule a call to the `QApplication.instance().quit()` function after 200 milliseconds, which means that the worker will quit after a certain amount of time has passed." + "type_name": "int|float", + "description": "Used to track the time elapsed since the last update of the graph, with the purpose of checking if it's been a certain amount of time (200ms) since the last update, and if so, quit the application." }, { "name": "compute", - "type_name": "instance", - "description": "Used to handle updates for node attributes. It takes two arguments: `id` which is the id of the node to be updated, and `attribute_names` which is a list of strings representing the names of the attributes to be updated." + "type_name": "Callable[[float,float],float]", + "description": "Used to compute the next node ID for the worker's node. It takes two arguments: the current time and a seed value, and returns the next node ID as a float value." }, { "name": "update_node_att", - "type_name": "event", - "description": "Called when a new node attribution is received from the graph. It updates the robot's odometry queue with the current advance, side, and angular speed and sets the `room_initialized` flag to false if the room ID changes." + "type_name": "Tuple[int,str,int,int]", + "description": "Used to update the attributes of a node in the graph when the node's ID matches the given ID. The attribute takes four arguments: the node ID, the attribute names, the old value, and the new value." }, { "name": "update_edge", - "type_name": "update_edge", - "description": "Used to update the attributes of an edge in a graph based on its type, such as \"current\" or \"RT\". It sets the translation and rotation of the robot based on the RT set." + "type_name": "Callable[float,float,str]", + "description": "Used to update the edge attributes of a graph based on specific conditions. It takes three arguments: the first is the source node ID, the second is the target node ID, and the third is the edge type. The attribute is called whenever a new edge is added or an existing edge is modified in the graph, and it can be used to set or update edge attributes based on specific conditions." }, { "name": "update_edge_att", - "type_name": "str", - "description": "Defined as a method that updates the attributes of an edge in the graph based on a specific edge type and a list of attribute names." + "type_name": "List[str]", + "description": "Used to update the attributes of an edge based on a specific type and name. It takes three parameters: the first is the id of the edge, the second is the type of edge, and the third is a list of attribute names to be updated." } ], "name": "SpecificWorker", @@ -433,28 +443,33 @@ "comment": null }, "item_type": "class", - "length": 416, + "length": 421, "docLength": null }, { - "id": "736bb328-b4ca-7b8b-5e4f-cb66525a0d81", + "id": "dff97ec1-552a-9187-a548-c33a4b688e14", "ancestors": [ - "db065633-a20c-b4a3-b546-23abf1d410fe" + "81834b2b-13a8-409b-7041-ea7aad5792ec" ], - "description": "Initializes a SpecificWorker object, setting its properties and connecting to signals for updating node attributes, edges, and edge attributes.", + "description": "Initializes the worker's internal state, including its graph, odometry queue, and other components essential for its operation.", "params": [ { "name": "proxy_map", - "type_name": "dict", - "description": "Used to specify a mapping from real-world coordinates to virtual coordinates for the robot's movement." + "type_name": "Dict[str, Any]", + "description": "Used to pass additional data to the SpecificWorker object." }, { "name": "startup_check", "type_name": "bool", - "description": "Used to run an initialization check on the graph when the agent starts up." + "description": "Used to check if the graph has been properly initialized before starting the worker's computation. If set to `False`, it will skip this check and proceed with the computation." } ], "returns": null, + "usage": { + "language": "python", + "code": "from SpecificWorker import SpecificWorker\n\n# Create an instance of the SpecificWorker class, passing in necessary arguments and initializing variables.\nproxy_map = {}\nworker = SpecificWorker(proxy_map)\n\n# Use the init method to initialize any necessary variables or perform any initialization actions that need to be done once when the object is created.\nworker.init()\n\n# Use the compute method to perform any computations required by the program and update any necessary variables.\nworker.compute()\n", + "description": "" + }, "name": "__init__", "location": { "start": 51, @@ -463,18 +478,23 @@ "indent": 8, "comment": null }, - "item_type": "method", + "item_type": "constructor", "length": 54, "docLength": null }, { - "id": "6c6431d9-1393-d3b9-5b46-3847c0c49dcb", + "id": "494ffcaf-17f0-9292-5440-6391c5fd6330", "ancestors": [ - "db065633-a20c-b4a3-b546-23abf1d410fe" + "81834b2b-13a8-409b-7041-ea7aad5792ec" ], - "description": "In the `SpecificWorker` class implements a robot's movement using ROS navigation stack, computing the robot's position and orientation based on the previous odometry data and adding it to the graph.", + "description": "Processes robot odometry data and updates the graph using G2O optimization to estimate the robot's position and orientation.", "params": [], "returns": null, + "usage": { + "language": "python", + "code": "# Example usage of the Python function compute\ndef main():\n # Create a new instance of the SpecificWorker class\n specific_worker = SpecificWorker(proxy_map)\n\n # Set some initial values for the worker's attributes\n specific_worker.agent_id = 20\n specific_worker.g = DSRGraph(0, \"G2O_agent\", specific_worker.agent_id)\n\n # Call the compute function with a few arguments\n specific_worker.compute(args=[\"--input-file\", \"/path/to/input_file.csv\"])\n\n# Call the main function to run the example code\nif __name__ == \"__main__\":\n main()\n", + "description": "" + }, "name": "compute", "location": { "start": 123, @@ -484,24 +504,29 @@ "comment": null }, "item_type": "method", - "length": 96, + "length": 101, "docLength": null }, { - "id": "1f7b5f1f-be86-5eb8-b24b-1aac542cd83a", + "id": "2140c48f-ed84-5b99-2545-c6c54962a658", "ancestors": [ - "db065633-a20c-b4a3-b546-23abf1d410fe" + "81834b2b-13a8-409b-7041-ea7aad5792ec" ], - "description": "Initializes a Graph2Online (G2O) graph for a specific robot's odometry data, by adding nominal corners and fixed poses to the graph based on room and door nodes in the environment map.", + "description": "1) initializes the g2o graph based on room and robot nodes, 2) adds nominal corners for rooms and doors, and 3) fixes the pose of a robot node using odometry information.", "params": [], "returns": { "type_name": "bool", - "description": "1 if the function was able to initialize the g2o graph successfully, and 0 otherwise." + "description": "1 if the g2o graph is successfully initialized with nominal corners and fixed poses, and 0 otherwise." + }, + "usage": { + "language": "python", + "code": "import SpecificWorker\n\n# Create a new instance of the worker class and call its initialize method with some arguments.\nworker = SpecificWorker(proxy_map=True, startup_check=False)\nworker.initialize_g2o_graph()\n\n# Some time later, use the g2o graph object to perform operations such as adding a new node or edge.\nworker.g2o.add_node(name='NewNode')\n", + "description": "" }, "name": "initialize_g2o_graph", "location": { - "start": 247, - "insert": 250, + "start": 252, + "insert": 255, "offset": " ", "indent": 8, "comment": null @@ -511,26 +536,31 @@ "docLength": null }, { - "id": "fb17fb15-9dd2-2aac-e149-6ea477cc49e5", + "id": "196e3e68-4c11-b38e-7b41-719b5edd6101", "ancestors": [ - "db065633-a20c-b4a3-b546-23abf1d410fe" + "81834b2b-13a8-409b-7041-ea7aad5792ec" ], - "description": "Calculates the displacement of an agent based on its odometry data, taking into account advancement, lateral movement, and angular movement.", + "description": "Calculates the displacement of the robot in three dimensions (lateral, forward, and angular) based on the odometry data stored in a queue.", "params": [ { "name": "odometry", - "type_name": "dict", - "description": "Passed as an argument to the function, containing the odometric data of the vehicle at each time step, including the position, velocity, and timestamp." + "type_name": "Tuple[float, float, float]", + "description": "A sequence of 3-element tuples representing the robot's position, velocity, and timestamp in a time interval." } ], "returns": { - "type_name": "3element", - "description": "3-D vector representing the displacement of a robot's end effector in terms of lateral, forward, and angular displacements." + "type_name": "Tuple[float,float,float]", + "description": "3 floating-point values representing the lateral displacement, forward displacement, and angular displacement of an object over a given time interval." + }, + "usage": { + "language": "python", + "code": "specific_worker = SpecificWorker(proxy_map=SomeProxyMap)\ndesplazamiento_lateral, desplazamiento_avance, desplazamiento_angular = specific_worker.get_displacement(odometry=SomeOdometry)\nprint(\"Displacement:\", displacement)\n", + "description": "" }, "name": "get_displacement", "location": { - "start": 452, - "insert": 453, + "start": 457, + "insert": 458, "offset": " ", "indent": 8, "comment": null @@ -540,26 +570,31 @@ "docLength": null }, { - "id": "3a44433e-c164-0bb0-a048-743c75fa20f0", + "id": "6c21ce38-1c27-049b-984f-6be77c359b75", "ancestors": [ - "db065633-a20c-b4a3-b546-23abf1d410fe" + "81834b2b-13a8-409b-7041-ea7aad5792ec" ], - "description": "Computes the covariance matrix of a set of vertices in a graph, using the gradient descent optimizer from the `g2o` library. It returns the computed covariance matrix and whether it was successfully computed or not.", + "description": "Computes the covariance matrix between a given vertex and all other vertices in the graph, using the G2O optimizer to compute marginals and upper triangle matrix representation.", "params": [ { "name": "vertex", - "type_name": "g2overtex", - "description": "Used to represent a specific vertex in a graph." + "type_name": "G2O.Vertex | G2O.HessianIndex", + "description": "Used to compute the covariance matrix for a specific vertex in the graph." } ], "returns": { - "type_name": "tuple", - "description": "2-element tuple containing two values: (`covariances_result`, `covariances`)." + "type_name": "numpyndarray", + "description": "2-dimensional and upper-triangular matrix representing the covariance matrix between a given vertex and all other vertices in the graph." + }, + "usage": { + "language": "python", + "code": "# Create an instance of SpecificWorker\nworker = SpecificWorker()\n\n# Call the get_covariance_matrix method with a vertex as argument\nvertex = g2o.VertexSE3(0) # Create a VertexSE3 object with index 0\nresult, cov_mat = worker.get_covariance_matrix(vertex)\n\n# Print the result and covariance matrix\nprint(f\"Result: {result}\")\nprint(f\"Covariance Matrix:\\n{cov_mat}\")\n", + "description": "" }, "name": "get_covariance_matrix", "location": { - "start": 471, - "insert": 472, + "start": 476, + "insert": 477, "offset": " ", "indent": 8, "comment": null @@ -569,23 +604,28 @@ "docLength": null }, { - "id": "889d2404-a66a-f3b6-8e47-4705c72d6823", + "id": "32bdf14a-da4a-c7ae-2443-b9488fb241d4", "ancestors": [ - "db065633-a20c-b4a3-b546-23abf1d410fe" + "81834b2b-13a8-409b-7041-ea7aad5792ec" ], - "description": "Visualizes the 3D positions of vertices and edges in a G2O file in real-time, using matplotlib. It loads the G2O file, calculates the positions of the vertices and edges, and plots them on a 3D scatter plot.", + "description": "Visualizes the real-time optimization process of a G2O algorithm by plotting the positions and edges of the vertices in 3D space.", "params": [ { "name": "optimizer", - "type_name": "instance", - "description": "Used to store an instance of the G2O optimizer class, which is responsible for minimizing the energy of the system." + "type_name": "object | Optimizer", + "description": "Used to load G2O files, estimate vertex positions, and visualize them in real-time." } ], "returns": null, + "usage": { + "language": "python", + "code": "from SpecificWorker import SpecificWorker\nimport matplotlib.pyplot as plt\n\n# Create a new instance of the SpecificWorker class\nworker = SpecificWorker()\n\n# Load a graph file and set up the G2OGraph object\nworker.g2o.load(\"archivo.g2o\")\n\n# Visualize the real-time optimization process using matplotlib's 3D plotting library\nplt.ion()\nfig = plt.figure()\nax = fig.add_subplot(111, projection='3d')\nworker.visualize_g2o_realtime(optimizer)\n", + "description": "" + }, "name": "visualize_g2o_realtime", "location": { - "start": 484, - "insert": 485, + "start": 489, + "insert": 490, "offset": " ", "indent": 8, "comment": null @@ -595,28 +635,33 @@ "docLength": null }, { - "id": "dade48c4-b3b7-33be-694f-984e420abf6c", + "id": "db934395-dc97-4b9f-7f41-4d42e8474534", "ancestors": [ - "db065633-a20c-b4a3-b546-23abf1d410fe" + "81834b2b-13a8-409b-7041-ea7aad5792ec" ], - "description": "Updates the attributes of a specific node in a graph, based on the current time and other factors.", + "description": "Updates the attributes of a node in a graph, specifically the odometry queue.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to represent the unique identifier of the node being updated, specifically the odometry node ID." + "description": "Represented as an integer value that identifies the node to be updated." }, { "name": "attribute_names", "type_name": "[str]", - "description": "An array of names of attributes to be updated on the node." + "description": "An array containing the names of attributes to be updated for the given node ID." } ], "returns": null, + "usage": { + "language": "python", + "code": "# Import SpecificWorker class from the module specific_worker.py\nfrom specific_worker import SpecificWorker\n\n# Initialize a SpecificWorker object named \"specific_worker\" with proxy map and startup check as True\nspecific_worker = SpecificWorker(proxy_map=True, startup_check=True)\n\n# Update node attribute using the update_node_att method\nspecific_worker.update_node_att(id=20, attribute_names=[\"robot_current_advance_speed\", \"robot_current_side_speed\", \"robot_current_angular_speed\"])\n", + "description": "\nIn this example, we initialize a SpecificWorker object with proxy map and startup check as True. Then, we use the update_node_att method to update the node attribute of the robot with ID 20 by passing in the attribute names that we want to update. The update_node_att method is then executed using the specific_worker object." + }, "name": "update_node_att", "location": { - "start": 521, - "insert": 530, + "start": 526, + "insert": 535, "offset": " ", "indent": 8, "comment": null @@ -626,28 +671,33 @@ "docLength": null }, { - "id": "ae1da25f-ceee-21ae-874c-7905cb623444", + "id": "adfc3c84-948f-4690-934e-0275aa1375a7", "ancestors": [ - "db065633-a20c-b4a3-b546-23abf1d410fe" + "81834b2b-13a8-409b-7041-ea7aad5792ec" ], - "description": "Updates an unspecified node with an ID and a type. If the type is \"corner\", it initializes a room graph if necessary. Otherwise, it does nothing.", + "description": "Updates a node with ID `id`. The method checks if the node type is \"corner\", and if so, it retrieves the room node from the graph and sets a flag indicating that the room has been initialized.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to represent the unique identifier for the node being updated." + "description": "Used to identify the node to be updated." }, { "name": "type", "type_name": "str", - "description": "Defined as \"corner\"." + "description": "Used to specify the node's type." } ], "returns": null, + "usage": { + "language": "python", + "code": "# Assuming you have created an instance of the SpecificWorker class, worker, as shown in the previous code snippet\nworker.update_node(id=100, type='corner') # Updates the node with ID 100 to be a corner\n", + "description": "\nThis example shows how the update_node function can be used to update the specific node with ID 100 to be of type 'corner'." + }, "name": "update_node", "location": { - "start": 537, - "insert": 547, + "start": 542, + "insert": 552, "offset": " ", "indent": 8, "comment": null @@ -657,23 +707,28 @@ "docLength": null }, { - "id": "8497841c-0cf0-3ca6-7a4c-1ada9b487ccd", + "id": "3e556671-8722-7790-9c4b-f6eb29ed8f0f", "ancestors": [ - "db065633-a20c-b4a3-b546-23abf1d410fe" + "81834b2b-13a8-409b-7041-ea7aad5792ec" ], - "description": "Deletes a node from a data structure managed by a `SpecificWorker` subclass of `GenericWorker`.", + "description": "Deletes a node from a list or dict based on its ID, setting `self.room_initialized` to `False`.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to identify the node to be deleted." + "description": "Intended to represent the unique identifier for the node being deleted." } ], "returns": null, + "usage": { + "language": "python", + "code": "# Create an instance of SpecificWorker class\nworker = SpecificWorker(proxy_map)\n\n# Call delete_node method with node id as parameter\nworker.delete_node(1234)\n", + "description": "" + }, "name": "delete_node", "location": { - "start": 549, - "insert": 550, + "start": 554, + "insert": 555, "offset": " ", "indent": 8, "comment": null @@ -683,33 +738,38 @@ "docLength": null }, { - "id": "f9896a12-dfdf-789b-f14f-bdb730087303", + "id": "ad7c5a47-c41a-8982-0d4b-b1573ab838c8", "ancestors": [ - "db065633-a20c-b4a3-b546-23abf1d410fe" + "81834b2b-13a8-409b-7041-ea7aad5792ec" ], - "description": "Updates the room ID and sets the `current_edge_set` attribute based on the type of edge received, and also performs RT translation and rotation if necessary.", + "description": "Updates the room ID and sets the current edge set based on the type of edge received from the graph.", "params": [ { "name": "fr", "type_name": "int", - "description": "Representative of the starting vertex of an edge in a graph." + "description": "Representing the starting node index of an edge in a graph." }, { "name": "to", "type_name": "int", - "description": "Used to represent the ID of the next node in the graph that the edge will be attached to." + "description": "The target node index of the edge to be updated, which can be either a room or the Shadow node." }, { "name": "type", "type_name": "str", - "description": "Used to indicate the type of edge being updated, either \"current\" or \"RT\"." + "description": "Used to specify the edge type, which can be either \"current\" or \"RT\"." } ], "returns": null, + "usage": { + "language": "python", + "code": "# Initialize the SpecificWorker class and its graph g\nspecific_worker = SpecificWorker(proxy_map=ProxyMap())\nspecific_worker.g = DSRGraph()\n\n# Add nodes to the graph\nnode1 = specific_worker.g.add_node(\"Node 1\")\nnode2 = specific_worker.g.add_node(\"Node 2\")\n\n# Add edges between the nodes\nedge = specific_worker.g.add_edge(node1, node2)\n\n# Call the update_edge function to set a new edge type\nspecific_worker.update_edge(fr=node1, to=node2, type=\"current\")\n\n# Update the graph with the new edge type\nspecific_worker.g.update()\n", + "description": "" + }, "name": "update_edge", "location": { - "start": 558, - "insert": 559, + "start": 563, + "insert": 564, "offset": " ", "indent": 8, "comment": null @@ -719,33 +779,38 @@ "docLength": null }, { - "id": "338c7aa8-6757-9cbe-9845-75ded7f5c60a", + "id": "e0e06e79-6b30-35bd-c340-bdc7b00ca599", "ancestors": [ - "db065633-a20c-b4a3-b546-23abf1d410fe" + "81834b2b-13a8-409b-7041-ea7aad5792ec" ], - "description": "Deletes an edge from a graph, specified by its ID and type.", + "description": "Deletes an edge from a graph, specified by its index (fr) and the index of its adjacent vertex (to). The edge's type can be specified for further filtering.", "params": [ { "name": "fr", "type_name": "int", - "description": "Passed as an argument to the function with the value of 123." + "description": "1st argument or input of the function indicating the first vertex or node to delete an edge from." }, { "name": "to", "type_name": "int", - "description": "Used to specify the target vertex ID for edge deletion." + "description": "Used to specify the destination vertex ID for the edge deletion operation." }, { "name": "type", "type_name": "str", - "description": "Used to specify the edge type to be deleted, which can be either 'weighted' or 'unweighted'." + "description": "Used to indicate the edge type to be deleted, with possible values 'in' or 'out'." } ], "returns": null, + "usage": { + "language": "python", + "code": "# Delete an edge from the graph\nworker.delete_edge(fr=1, to=2, type=\"TYPE\")\n", + "description": "\nIn this example, the `delete_edge` method of the SpecificWorker class is called with three arguments: fr, to, and type. The first two arguments, fr and to, represent the source and target nodes of the edge that should be deleted, respectively. The third argument, type, specifies the type of the edge that should be deleted.\n\nFor instance, if we want to delete an edge with the source node being node 1 and the target node being node 2, and the edge has a type of \"TYPE\", we can call the `delete_edge` method as follows:\n" + }, "name": "delete_edge", "location": { - "start": 586, - "insert": 587, + "start": 591, + "insert": 592, "offset": " ", "indent": 8, "comment": null @@ -1158,137 +1223,127 @@ "path": "agents/long_term_spatial_memory_agent/src/specificworker.py", "content": { "structured": { - "description": "a class `Room` that represents a room in a graphical user interface (GUI) and performs various operations on it. The class has several methods:\n\n* `insert_room_node`: Inserts a new room node into the graph.\n* `update_edge`: Updates an edge in the graph, setting its type to \"current\" if necessary.\n* `delete_node`: Deletes a room node from the graph.\n* `update_edge_att`: Updates an edge attribute in the graph.\n* `delete_edge`: Deletes an edge from the graph.\n\nThe code uses the `igraph` package to interact with the graph, and the `pickle` package for serializing and deserializing the graph. The class also has some console print statements to display messages during execution.", + "description": "An igraph module that simulates a robot's navigation through a room using RT graph theory. It provides several functions for inserting, updating, and deleting nodes and edges in the room graph, as well as querying node and edge attributes. The code also includes logic to set the current state of the robot based on the type of edge inserted or removed from the room graph. The module uses the `igraph` package to manipulate graphs and perform graph theory operations.", "items": [ { - "id": "d14b1fa8-9d9a-61a7-184c-3f4e98189154", + "id": "8012c732-b5dd-93a9-2441-d47f31f3d2fa", "ancestors": [], - "description": "Manages a graph representing a robot's movement and interactions with its environment, performing various operations such as inserting, updating, and deleting nodes and edges, as well as tracking the robot's state.", + "description": "Manages a graph representing a manipulation task, providing methods for inserting, updating, and deleting nodes and edges. It also keeps track of the robot's current state and performs actions based on that state.", "attributes": [ { "name": "Period", - "type_name": "str", - "description": "Used to store the current time period in which the worker is active, allowing for more efficient handling of tasks during different time slots." + "type_name": "str|int", + "description": "Used to store the period of time during which the worker performs its task, either in seconds or minutes, depending on the value assigned to it." }, { "name": "agent_id", - "type_name": "int", - "description": "Used as the ID of the agent that owns or manipulates the graph, which could be a robot or a human operator." + "type_name": "int|str", + "description": "Used to store the ID of the specific worker for which the methods of this class are intended to work." }, { "name": "g", "type_name": "igraphGraph", - "description": "Used for manipulating the graph in various methods like `insert_igraph_vertex`, `insert_igraph_edge`, etc." - }, - { - "name": "update_node", - "type_name": "int", - "description": "Used to update a node's attributes based on its ID. It takes two arguments: `id` (int) and `type` (str), where `id` is the node's ID to be updated and `type` represents the type of update (e.g., \"door\", \"room\")." - }, - { - "name": "update_edge", - "type_name": "int", - "description": "Used to update the edge of a robot moving from one node to another with a certain type (current) when there is no current edge and a room node exists." + "description": "Used for handling long-term dependencies between nodes in the graph." }, { "name": "startup_check", - "type_name": "str", - "description": "Used to check if the worker is in startup mode or not. It is set to \"true\" when the worker starts up and \"false\" otherwise." + "type_name": "bool|str", + "description": "Used to check if the worker has started successfully or not. It can be set to a specific value like \"success\" or \"failure\" to indicate whether the startup was successful or not, or it can be left blank to default to a boolean value indicating whether the worker started successfully or not." }, { "name": "rt_api", - "type_name": "str", - "description": "Represented as a string value indicating which RT (Real-Time) affordance to use." + "type_name": "str|int", + "description": "Used to store the RT (Real-Time) API of the worker, which allows the robot to perform real-time actions." }, { "name": "inner_api", - "type_name": "instance", - "description": "A Python method that returns the inner API of the worker. It is used to define the functionality of the worker and its interactions with other components of the system." + "type_name": "Dict[str,Any]", + "description": "Used to store internal APIs for the worker's methods that are not exposed to the outside world." }, { "name": "robot_name", - "type_name": "str", - "description": "Used to store the name of the robot being controlled by the worker." + "type_name": "str|List[str]", + "description": "Used to store the name of the robot that the worker belongs to." }, { "name": "robot_id", "type_name": "int", - "description": "Used to identify the robot node in the graph." + "description": "Used to represent the unique identifier of the robot node in the graph." }, { "name": "last_robot_pose", - "type_name": "IGraph", - "description": "Used to store the last known pose of the robot, which can be used for debugging purposes or to track the robot's movement." + "type_name": "Dict[str,float]", + "description": "Used to store the last known pose of the robot before it entered a room or crossed a boundary." }, { "name": "robot_exit_pose", - "type_name": "RT", - "description": "A dictionary containing the pose (position and orientation) of the robot when it exits the room through the door." + "type_name": "str|int", + "description": "Used to store the ID of the room node that the robot exits through when transitioning from a door node to a room node." }, { "name": "state", - "type_name": "str", - "description": "Used to store the current state of the worker, which can be either \"idle\", \"working\", or \"crossed\"." + "type_name": "str|bool", + "description": "Used to keep track of the worker's current state (either \"idle\", \"crossed\", or \"completed\")." }, { "name": "affordance_node_active_id", - "type_name": "int", - "description": "Used to store the ID of the affordance node that is currently active. It is used to check if the affordance node is completed and not active, and to switch to the crossed state when it is completed and not active." + "type_name": "int|bool", + "description": "Used to store the ID of the affordance node that is currently active or not." }, { "name": "exit_door_id", - "type_name": "int", - "description": "1234, which represents the ID of the door node that serves as the exit of a room." + "type_name": "int|str", + "description": "Used to store the ID of the door node that serves as an exit from a room." }, { "name": "room_exit_door_id", - "type_name": "int", - "description": "Used to represent the ID of the door node that leads from a room to the outside world in the graph." + "type_name": "int|str", + "description": "16 by default, representing the id of a specific door node in the long-term graph that marks the exit of a room." }, { "name": "enter_room_node_id", - "type_name": "int", - "description": "0 by default, representing the ID of the room node that the worker enters when it completes its task." - }, - { - "name": "graph", - "type_name": "igraphGraph", - "description": "Used for storing and manipulating the graph represented by the worker. It contains the nodes, edges, and other attributes of the graph." + "type_name": "int|str", + "description": "Represented as a room number that, when updated to a value greater than zero, indicates that the robot has entered a new room." }, { "name": "vertex_size", - "type_name": "int", - "description": "Used to store the size of the vertex (node) in the graph, indicating the number of attributes associated with each node." + "type_name": "int|str", + "description": "Used to store the size or dimension of a vertex in the graph, which can be used to determine the size of a node in the graph." }, { "name": "not_required_attrs", - "type_name": "list", - "description": "Used to keep track of a list of attributes that are not required for the worker's functionality, but can be useful for debugging or other purposes." + "type_name": "List[str]", + "description": "Used to store a list of attributes that are not required for the worker's operations. It helps to optimize the worker's performance by ignoring these unused attributes during updates, inserts, or deletes." }, { "name": "long_term_graph", "type_name": "igraphGraph", - "description": "Used for storing the long-term graph of the environment, which is updated when certain events occur like inserting or deleting nodes." - }, - { - "name": "global_map", - "type_name": "igraphGraph", - "description": "Used to store the global map of the environment, which is a graph representation of the environment that includes all nodes and edges." + "description": "Used to store the long-term graph representation of the environment, which is updated during the worker's execution." }, { "name": "insert_current_edge", - "type_name": "igraphEdge", - "description": "Used to insert a new edge into the graph with the given source and target nodes, and the specified edge type." + "type_name": "NoneOrEdge|List[str]", + "description": "Used to insert a new edge into the graph with the specified fr and to nodes, and sets it as the current edge if no current edge exists." }, { "name": "timer", - "type_name": "int", - "description": "Set to the number of milliseconds since the epoch (January 1, 1970, 00:00:00 UTC) when the worker was created. It is used to track the elapsed time for the worker's tasks." + "type_name": "int|float", + "description": "Used to track the time taken by the worker to process a task." }, { "name": "compute", - "type_name": "instance", - "description": "Used to compute the shortest path between two nodes in the graph based on the RT model." + "type_name": "Callable[[str,str],Any]", + "description": "Used to specify a function that computes the next action for the robot based on its current state and the environment." + }, + { + "name": "update_node", + "type_name": "Dict[str,Any]", + "description": "Used to update a node in the graph based on its ID, type (door or room), and name. It performs various actions such as checking if a door node exists, inserting a door node, updating the RT translation and rotation, and drawing the graph." + }, + { + "name": "update_edge", + "type_name": "Union[str,int]", + "description": "Used to update the edge attributes of a specific robot node in the graph when the node's door is opened." } ], "name": "SpecificWorker", @@ -1300,28 +1355,33 @@ "comment": null }, "item_type": "class", - "length": 572, + "length": 600, "docLength": null }, { - "id": "768be6cd-7a26-04b8-5c48-d827e00b1c97", + "id": "2420b0f0-67bd-b096-4c4c-57bbc351c392", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Initializes an instance of the SpecificWorker class, setting up graph connections and variables for storing information about a robot's environment and state.", + "description": "Initializes instance variables and sets up event listeners for graph updates, timeouts, and node measurements.", "params": [ { "name": "proxy_map", - "type_name": "igraphGraph", - "description": "Used to represent the graph mapping between nodes and objects, allowing for efficient object manipulation." + "type_name": "Dict[str, Any]", + "description": "Used to pass additional data to the SpecificWorker object. It contains key-value pairs that can be used to store any information required by the worker." }, { "name": "startup_check", "type_name": "bool", - "description": "Used to check if the agent has already been started." + "description": "Used to check if the agent has already started its computation, skipping the computation if it has already been done." } ], "returns": null, + "usage": { + "language": "python", + "code": "from SpecificWorker import SpecificWorker\n\n# Initialize a new instance of the class\nspecific_worker = SpecificWorker(proxy_map)\n", + "description": "" + }, "name": "__init__", "location": { "start": 50, @@ -1330,31 +1390,36 @@ "indent": 8, "comment": null }, - "item_type": "method", - "length": 46, + "item_type": "constructor", + "length": 51, "docLength": null }, { - "id": "b75c11a0-d78b-c099-0542-820638964df9", + "id": "3cc990dd-edc6-eaa3-e84f-526e79602e4f", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Sets the parameters of an object, then removes a self-edge from a room and stores the ID of an exit door in a variable. It also assigns attributes to both doors in the new room.", + "description": "Sets the parameters passed as an argument to `True`. Then, it removes a self-edge from the current room, stores the ID of the exit door in a variable, and sets the name attribute of both doors to the other side door's name.", "params": [ { "name": "params", - "type_name": "object", - "description": "Used to store any relevant data or configuration for the function's operation." + "type_name": "bool", + "description": "Passed to set parameters for an object of class `Room`." } ], "returns": { - "type_name": "Boolean", + "type_name": "bool", "description": "True." }, + "usage": { + "language": "python", + "code": "# Initialise a SpecificWorker instance and assign its parameters\nworker = SpecificWorker(proxy_map)\nparams = {\"Period\": 100, \"agent_id\": 13}\nworker.setParams(params)\n\n# Use the worker instance to perform some tasks\nworker.startup_check()\nworker.rt_api.update_node_attributes(worker.g, node_id=worker.robot_id, attr_map={\"timestamp_alivetime\": 1})\n", + "description": "\nIn this example, we first create a new SpecificWorker instance and initialise its parameters using the setParams function. We then use the worker instance to perform some tasks such as startup_check() and updating node attributes with the rt_api object." + }, "name": "setParams", "location": { - "start": 116, - "insert": 117, + "start": 124, + "insert": 125, "offset": " ", "indent": 8, "comment": null @@ -1364,57 +1429,72 @@ "docLength": null }, { - "id": "bd530f7b-1d60-7699-5c46-38a9649a60a7", + "id": "d6fad876-0520-74ba-db4f-01fdb0bda8d3", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Determines the current state of a worker and performs the appropriate actions based on that state, including idle, crossing, crossed, initializing room, known room, initializing doors, storing graph, and removing.", + "description": "Computes and updates the robot's RT (reactive transport) based on the current state of the graph.", "params": [], "returns": null, + "usage": { + "language": "python", + "code": "import SpecificWorker\n\n# Initialize the SpecificWorker object with the proxy map and a flag for startup check\nspecific_worker = SpecificWorker(proxy_map, startup_check=True)\n\n# Call the compute method to perform the computation\nresult = specific_worker.compute()\n\n# Print the result of the computation\nprint(result)\n", + "description": "" + }, "name": "compute", "location": { - "start": 130, - "insert": 149, + "start": 138, + "insert": 141, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 24, + "length": 61, "docLength": null }, { - "id": "75ed2888-b3fb-1cbf-ce45-8c960f6d2330", + "id": "8506fabb-8dc7-2aab-f04a-6f0ba0f9cc92", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Performs the following tasks:\n\n1. Identifies active affordance nodes in the short-term graph.\n2. Checks if any \"current\" edge exists in the short-term graph.\n3. If a door is found, computes its pose in the long-term graph and associates it with the current room.", + "description": "Computes the affordance pose of the robot in a new room based on its current position and orientation, and associates doors in the long-term graph with their corresponding rooms.", "params": [], "returns": null, + "usage": { + "language": "python", + "code": "specific_worker = SpecificWorker(proxy_map) # Initiate a specific worker object\nspecific_worker.idle() # Call the idle method from the specific worker object\n", + "description": "" + }, "name": "idle", "location": { - "start": 168, - "insert": 170, + "start": 211, + "insert": 213, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 76, + "length": 75, "docLength": null }, { - "id": "09e1943c-c650-459e-6f49-e8c99cf5b03f", + "id": "04e2af7d-eddb-8f8c-6743-11e3dda21651", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Determines the current room based on an active affordance node and updates the state machine accordingly. If the node has no parent, it sets the exit door ID to the value of the active affordance node's parent attribute.", + "description": "Updates the state of the worker based on the active affordance node and exit door ID.", "params": [], "returns": null, + "usage": { + "language": "python", + "code": "# create an instance of the SpecificWorker class\nworker = SpecificWorker(proxy_map)\n\n# set the agent's ID and robot name\nagent_id = 13\nrobot_name = \"Shadow\"\n\n# call the crossed method\nworker.crossed()\n", + "description": "\nThe crossed function is a method of the SpecificWorker class that subclasses GenericWorker. It takes no arguments and does not return any value. The function first gets the affordance node with ID self.affordance_node_active_id from the graph g using get_node(). If this node has a parent attribute, it sets the exit door ID to the parent attribute value. If the parent attribute is empty, it returns.\nIf the exit door ID is set, the function deletes an edge between room_exit_door_id and self.room_exit_door_id using delete_edge().\nThe function then gets the exit door node with ID self.exit_door_id from the graph g using get_node(). If this node has a connected_room_name attribute, it sets the state to known_room and prints an INSERTING KNOWN ROOM message. If the exit door ID is empty, it sets the state to initializing_room and prints an INITIALIZING ROOM message.\nNote that the SpecificWorker class also has other attributes such as agent_id, g, rt_api, inner_api, robot_name, last_robot_pose, robot_exit_pose, state, affordance_node_active_id, exit_door_id, room_exit_door_id, enter_room_node_id, and vertex_size." + }, "name": "crossed", "location": { - "start": 269, - "insert": 271, + "start": 310, + "insert": 312, "offset": " ", "indent": 8, "comment": null @@ -1424,17 +1504,22 @@ "docLength": null }, { - "id": "d5bde5e7-2579-51a6-ca41-9f515472bf25", + "id": "789455c6-28e2-7985-854d-e4841c68865e", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "1) identifies room nodes in the graph, 2) sets the `enter_room_node_id` field, and 3) enters the \"initializing doors\" state.", + "description": "Initializes the room nodes and sets the enter room node ID, then transitions to the \"initializing doors\" state.", "params": [], "returns": null, + "usage": { + "language": "python", + "code": "# Initialize the specific worker class with the proxy map and the startup check flag set to False\nspecific_worker = SpecificWorker(proxy_map, startup_check=False)\n\n# Call the initializing_room method on the specific worker instance\nspecific_worker.initializing_room()\n", + "description": "" + }, "name": "initializing_room", "location": { - "start": 289, - "insert": 292, + "start": 330, + "insert": 333, "offset": " ", "indent": 8, "comment": null @@ -1444,17 +1529,22 @@ "docLength": null }, { - "id": "0abb8428-4ef9-1099-494a-164dd3beb89f", + "id": "423155ab-cc3e-9ab0-2342-a93d7e99b214", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Within the `SpecificWorker` class defines how a robot navigates through a graph representation of a maze to reach an objective room while avoiding obstacles and taking into account the associated door's rotation.", + "description": "Determines the room name associated with a door based on its ID and updates the graph with new edges and nodes to simulate robot movement through the door and into the adjacent room.", "params": [], "returns": null, + "usage": { + "language": "python", + "code": "# Initialize a SpecificWorker object with the proxy map\nspecific_worker = SpecificWorker(proxy_map)\n\n# Call the known_room method on the specific worker object\nspecific_worker.known_room()\n", + "description": "" + }, "name": "known_room", "location": { - "start": 303, - "insert": 305, + "start": 344, + "insert": 346, "offset": " ", "indent": 8, "comment": null @@ -1464,17 +1554,22 @@ "docLength": null }, { - "id": "11fcaca4-6eb8-0f92-fe46-b59555cb21f2", + "id": "20438ef8-46d9-6a88-2346-a153824233c9", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Identifies doors connected to a specified exit door and associates them with each other, updating node attributes and the state of the worker.", + "description": "1) retrieves exit edges and matching doors, 2) sets other side door name and connected room name attributes for each exit door node, and 3) associates doors using their names and connected rooms.", "params": [], "returns": null, + "usage": { + "language": "python", + "code": "SpecificWorker(proxy_map).initializing_doors()\n", + "description": "\nThis code will call the method \"initializing_doors\" of the class SpecificWorker. The method takes no arguments, and it is part of a class that inherits from GenericWorker, so we need to pass an instance of a GenericWorker to the method. In this example, we are passing proxy_map as argument." + }, "name": "initializing_doors", "location": { - "start": 397, - "insert": 399, + "start": 438, + "insert": 440, "offset": " ", "indent": 8, "comment": null @@ -1484,28 +1579,33 @@ "docLength": null }, { - "id": "64841fe3-a4e4-5e90-864a-db8fe9332e9f", + "id": "f8912fcb-f70e-d0a1-424e-4a48496ff5f8", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "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.", + "description": "Connects two doors in a graph by adding an edge between their nodes and updates their attributes to reflect the connection.", "params": [ { "name": "door_1", "type_name": "str", - "description": "A string representing the name of the first door to be associated with another door in the graph." + "description": "A list containing the name of the first door to be associated with the long-term graph." }, { "name": "door_2", "type_name": "str", - "description": "Represented as an igraph vertex object, which contains information about the door node to be associated with the first door node passed as input." + "description": "A name of another door in the graph, which will be connected to the current door node through an edge in the graph." } ], "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\ndoor_1 = (\"door_name\", \"connected_room_name\")\ndoor_2 = (\"other_side_door_name\", \"connected_room_name\")\nworker.associate_doors(door_1, door_2)\n", + "description": "\nIn this example, the user is creating a new instance of the SpecificWorker class and then invoking the associate_doors method by passing in two doors (door_1 and door_2). The method will add an edge between the two nodes in the graph representing each door, as well as set the \"other_side_door_name\" and \"connected_room_name\" attributes for both nodes." + }, "name": "associate_doors", "location": { - "start": 436, - "insert": 438, + "start": 477, + "insert": 479, "offset": " ", "indent": 8, "comment": null @@ -1515,17 +1615,22 @@ "docLength": null }, { - "id": "d14e9d2c-fd3b-9c95-d54d-82acccfc7009", + "id": "a8d529d0-b6f1-f692-b541-cfd55b35f825", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Within SpecificWorker, a subclass of GenericWorker, stores a graph representation of a room and its exits in a file called \"graph.pkl\".", + "description": "Saves the graph representation of a room to a file named \"graph.pkl\" using Python's pickle module.", "params": [], "returns": null, + "usage": { + "language": "python", + "code": "# Example usage of store_graph function\nworker = SpecificWorker(proxy_map=ProxyMap(), startup_check=False)\nroom_node_id = -1 # Room node id\nworker.store_graph(room_node_id)\n", + "description": "\nIn this example, the user creates an instance of the `SpecificWorker` class and calls the `store_graph` method with a room node ID as an argument. The function then tries to find the corresponding room node in the igraph object using the `find` method and saves it to a file if it is not found." + }, "name": "store_graph", "location": { - "start": 455, - "insert": 456, + "start": 496, + "insert": 497, "offset": " ", "indent": 8, "comment": null @@ -1535,17 +1640,22 @@ "docLength": null }, { - "id": "b4c71ece-c0c2-6f93-814a-f62b2e37eac0", + "id": "0cdf2d45-fad8-44a8-c74d-90a9b7d7fe2d", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Removes edges from the graph that connect nodes representing rooms, based on their room numbers. It first identifies edges with room numbers matching those of the current room, then deletes them and updates a dictionary for future reference. Finally, it draws the updated graph and sets the state to \"idle\".", + "description": "Removes edges from the long-term graph based on room numbers and updates the state of the worker.", "params": [], "returns": null, + "usage": { + "language": "python", + "code": "# Create an instance of SpecificWorker\nspecific_worker = SpecificWorker(proxy_map)\n\n# Call the removing method with a specific node as its argument\nspecific_worker.removing(200)\n", + "description": "\nIn this example, we create an instance of `SpecificWorker` and then call the `removing` method with a specific node as its argument. The function removes all nodes connected to the specified node by any edge with the \"RT\" type. It also removes all edges of the \"has\" type that have the specified node as their origin or destination, and deletes the specified node." + }, "name": "removing", "location": { - "start": 469, - "insert": 471, + "start": 510, + "insert": 512, "offset": " ", "indent": 8, "comment": null @@ -1555,23 +1665,28 @@ "docLength": null }, { - "id": "8162c2ce-cb2a-1689-a149-3b886804fd98", + "id": "24746b75-48b7-4bb6-1b42-dbcab50aabd8", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Traverses a graph, starting from a given node, and performs a specific operation (in this case, inserting vertices and edges) on the nodes and edges it encounters.", + "description": "Traverses a graph by starting at a designated node and following RT edges to reach other nodes, inserting vertices and edges into an IGraph object as it goes.", "params": [ { "name": "node_id", "type_name": "int", - "description": "Used to identify the node that the function operates on." + "description": "Used to represent the unique identifier for a node in the graph." } ], "returns": null, + "usage": { + "language": "python", + "code": "# Create a SpecificWorker instance with proxy_map and startup_check arguments set to True\nspecific_worker = SpecificWorker(proxy_map, startup_check=True)\n\n# Traverse the graph starting from the robot node\nspecific_worker.traverse_graph(\"Robot\")\n", + "description": "\nThis example uses the traverse_graph function of the SpecificWorker class to traverse a graph starting from the \"Robot\" node." + }, "name": "traverse_graph", "location": { - "start": 495, - "insert": 497, + "start": 536, + "insert": 538, "offset": " ", "indent": 8, "comment": null @@ -1581,23 +1696,28 @@ "docLength": null }, { - "id": "de268dea-32ae-40ac-af42-325662be034d", + "id": "500a6fa2-405e-aa91-9540-a1c44ea71e8e", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Traverses the graph, starting from a given node, and inserts vertices and edges into a DSR graph based on certain conditions.", + "description": "Traverses the graph by visiting all vertices that have successors in the long-term graph, and for each such vertex, it checks if its successor has a higher level than the current vertex, and if so, it inserts a DSR vertex and edge between them, and recursively traverses the successor.", "params": [ { "name": "node", - "type_name": "igraphVertex", - "description": "Represented by an index in the graph." + "type_name": "igraph.Vertex", + "description": "Used to represent the current node being traversed." } ], "returns": null, + "usage": { + "language": "python", + "code": "graph = SpecificWorker(proxy_map)\nstarting_node = graph.g.get_node(\"MyStartingNode\")\ngraph.traverse_igraph(starting_node)\n", + "description": "\nThe code creates an instance of the SpecificWorker class and passes a proxy map object to the constructor. Then, it retrieves a starting node from the graph using the g.get_node method and uses this node as the argument for the traverse_igraph function. The function then recursively traverses the graph, inserting DSR vertices and edges based on the condition in the if statement." + }, "name": "traverse_igraph", "location": { - "start": 505, - "insert": 506, + "start": 546, + "insert": 547, "offset": " ", "indent": 8, "comment": null @@ -1607,54 +1727,64 @@ "docLength": null }, { - "id": "e91753f5-7b0b-869b-e440-6401584da274", + "id": "60d7e0f3-5642-a39a-4e40-66246d4c4948", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Adds a vertex to an igraph graph and populates its attributes based on the given node's attributes. It also attempts to find an edge connecting the new vertex to another vertex with a matching \"other_side_door_name\" attribute, and adds that edge if found.", + "description": "Adds a new vertex to an existing graph, updating its attributes and edges based on node-specific data.", "params": [ { "name": "node", - "type_name": "igraphNode", - "description": "Passed in to add the vertex to the graph." + "type_name": "igraph.Node", + "description": "Used to insert a new vertex into the graph." } ], "returns": null, + "usage": { + "language": "python", + "code": "# create a new instance of the SpecificWorker class and pass in proxy_map as argument\nspecific_worker = SpecificWorker(proxy_map)\n\n# add a vertex to the long term graph with the name \"MyVertex\"\nspecific_worker.insert_igraph_vertex(\"MyVertex\")\n", + "description": "\nIn this example, we first create a new instance of the SpecificWorker class and pass in proxy_map as an argument. We then use the insert_igraph_vertex function to add a vertex with the name \"MyVertex\" to the long term graph." + }, "name": "insert_igraph_vertex", "location": { - "start": 518, - "insert": 519, + "start": 559, + "insert": 560, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 28, + "length": 29, "docLength": null }, { - "id": "d8e8572a-c8bd-9f80-3041-8a09b43308a2", + "id": "7467a4df-2da4-a89d-dc40-8d0db7be370f", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Takes in a parent node name and a node object, creates a new node with the appropriate attributes, and inserts it into the graph using the `GenericWorker` class's `insert_node` method.", + "description": "Inserts a new node into a graph, updating the parent node's attribute with the agent ID and copying over non-required attributes from the input node to the new node.", "params": [ { "name": "parent_name", "type_name": "str", - "description": "Used to identify the parent node for the new vertex being inserted." + "description": "Used to specify the name of the parent node to which the new node will be added." }, { "name": "node", - "type_name": "Node", - "description": "Represented as a Python dictionary containing the node's attributes and values, such as agent ID, type, and name." + "type_name": "Node | dict", + "description": "Passed the attributes and data of a vertex to be inserted into a graph." } ], "returns": null, + "usage": { + "language": "python", + "code": "# Create a new instance of SpecificWorker class and call its constructor\nmy_worker = SpecificWorker(proxy_map)\n\n# Use the worker to add a vertex to the graph\nmy_worker.insert_dsr_vertex(\"parent_node\", {\"type\": \"room\", \"name\": \"Living Room\"})\n", + "description": "" + }, "name": "insert_dsr_vertex", "location": { - "start": 553, - "insert": 555, + "start": 594, + "insert": 596, "offset": " ", "indent": 8, "comment": null @@ -1664,54 +1794,64 @@ "docLength": null }, { - "id": "b9e4cd38-45f4-25b0-1543-8069bb5dc584", + "id": "ac75a866-f308-2a90-484f-864828cce9c4", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Modifies an existing graph by adding a new edge between two nodes based on edge attributes.", + "description": "Adds an edge to a long-term graph based on a given edge attribute, updating the node positions and rotation accordingly.", "params": [ { "name": "edge", - "type_name": "igraphEdge", - "description": "Passed as an instance of the Edge class, representing an edge in the graph with specified attributes." + "type_name": "rt.Edge", + "description": "Passed by reference to the method, representing an edge with attributes such as translation and rotation." } ], "returns": null, + "usage": { + "language": "python", + "code": "# Create an instance of SpecificWorker\nmy_worker = SpecificWorker()\n\n# Create an edge object with attributes \"origin\" and \"destination\"\nedge = Edge(origin=\"node1\", destination=\"node2\")\n\n# Call the insert_igraph_edge method on my_worker with the edge as an argument\nmy_worker.insert_igraph_edge(edge)\n", + "description": "\nIn this example, an instance of SpecificWorker is created and an Edge object is created with attributes \"origin\" and \"destination\". The insert_igraph_edge method of SpecificWorker is then called with the Edge object as an argument, which adds a new edge to the graph." + }, "name": "insert_igraph_edge", "location": { - "start": 567, - "insert": 568, + "start": 608, + "insert": 609, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 9, + "length": 12, "docLength": null }, { - "id": "658729f3-ea08-1cab-dd46-c1207d76fc8d", + "id": "35a1d65c-21d4-f7be-c446-071ce717b9a2", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Modifies an edge in a graph based on input arguments 'org' and 'dest'. It creates a new edge with the appropriate RT value and rotation, and inserts it into the graph.", + "description": "Modifies an existing graph by adding an edge with specific RT attributes, based on input org and dest nodes.", "params": [ { "name": "org", - "type_name": "Agent", - "description": "Used to represent the starting node of the edge." + "type_name": "GraphNode | NoneType", + "description": "Used to represent the origin node of the edge to be inserted. If it is None, the function will use the root node as the origin." }, { "name": "dest", - "type_name": "Agent", - "description": "Used to represent the destination node of the edge being inserted." + "type_name": "Node | str", + "description": "Used to specify the destination node or name in the graph for insertion of an edge." } ], "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\n# Create a new edge between two nodes in the graph\nworker.insert_dsr_edge(\"node1\", \"node2\")\n", + "description": "\nIn this example, the insert_dsr_edge function is called on an instance of the SpecificWorker class (which inherits from GenericWorker). The function takes in two arguments: org and dest. Org represents the origin node of the edge, and dest represents the destination node. The function first checks if org is None. If it is not None, then it retrieves the edge ID between org and dest using g.get_eid() and retrieves the attributes of the edge using g.es[edge_id]. It then creates a new Edge object with the destination node ID as its origin, the origin node ID as its destination, and an RT attribute with a value of 0 (as this is not a realistic use case). It then assigns the RT translation and rotation attributes to the new edge using new_edge.attrs[\"rt_translation\"] = Attribute(np.array([0, 0, 0], dtype=np.float32), self.agent_id) and new_edge.attrs[\"rt_rotation_euler_xyz\"] = Attribute(np.array([0, 0, 0], dtype=np.float32), self.agent_id). Finally, it inserts the new edge into the graph using g.insert_or_assign_edge()." + }, "name": "insert_dsr_edge", "location": { - "start": 581, - "insert": 584, + "start": 625, + "insert": 628, "offset": " ", "indent": 8, "comment": null @@ -1721,46 +1861,31 @@ "docLength": null }, { - "id": "68515eb1-870c-66a0-e74d-fc58b5dc6b6c", - "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" - ], - "description": "Draws the graph based on the layout \"kamada_kawai\", plots edges, and displays node names using annotations.", - "params": [], - "returns": null, - "name": "draw_graph", - "location": { - "start": 611, - "insert": 612, - "offset": " ", - "indent": 8, - "comment": null - }, - "item_type": "method", - "length": 16, - "docLength": null - }, - { - "id": "ffe7cb5a-2c7c-f8ab-fc4a-6a26dd7fd44b", + "id": "d927aae0-9021-6196-9b41-95f639864b45", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Within SpecificWorker, a subclass of GenericWorker, retrieves the room ID associated with a given node ID by querying the attribute \"room_id\" and returning its value upon success or -1 on error.", + "description": "Determines the room number of an element by retrieving the attribute 'room_id' from the element's node ID.", "params": [ { "name": "node_id", "type_name": "str", - "description": "Passed as an argument to the function, representing the unique identifier of a Node in the graph." + "description": "Used to reference a node in the graph." } ], "returns": { "type_name": "int", "description": "The room ID associated with a given node ID." }, + "usage": { + "language": "python", + "code": "# Initialize the SpecificWorker class and its attributes\nspecific_worker = SpecificWorker()\n\n# Get the room number of a node with ID \"my_node\"\nroom_id = specific_worker.check_element_room_number(\"my_node\")\nprint(f\"The room number of node 'my_node' is {room_id}\")\n", + "description": "" + }, "name": "check_element_room_number", "location": { - "start": 636, - "insert": 637, + "start": 655, + "insert": 656, "offset": " ", "indent": 8, "comment": null @@ -1770,26 +1895,31 @@ "docLength": null }, { - "id": "2288e20d-9011-75bc-464b-b4c8b38be6ad", + "id": "5ce9e553-2b0d-88b8-bf49-7882013c2d15", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Within the `SpecificWorker` class, checks the \"level\" attribute of a specified node in the graph and returns its value. If the attribute is not found, it prints an error message and returns -1.", + "description": "Determines the element level of a node based on its attribute \"level\" and returns it if found, or -1 otherwise.", "params": [ { "name": "node_id", - "type_name": "str", - "description": "Used to identify a node in the graph." + "type_name": "int", + "description": "Represented by the identifier `node_id`. It serves as an index into the graph's node list to access the specified node." } ], "returns": { "type_name": "int", - "description": "Element level of the node with the given ID" + "description": "Element level of the node with given id" + }, + "usage": { + "language": "python", + "code": "# Import SpecificWorker class from GenericWorker module\nfrom GenericWorker import SpecificWorker\n\n# Initialize a new SpecificWorker object\nmy_specific_worker = SpecificWorker(\"proxy_map\")\n\n# Call the check_element_level method with the desired node ID as an argument\nelement_level = my_specific_worker.check_element_level(1234)\n\n# Print the element level value to the console\nprint(f\"Element level of node 1234 is {element_level}\")\n", + "description": "" }, "name": "check_element_level", "location": { - "start": 645, - "insert": 646, + "start": 664, + "insert": 665, "offset": " ", "indent": 8, "comment": null @@ -1799,23 +1929,28 @@ "docLength": null }, { - "id": "e14126be-ce12-ec84-1e4f-5b15b353df35", + "id": "ca797b1f-3a72-7887-b54c-bcac0d9c7872", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Within the SpecificWorker class retrieves the room ID of a given node, then checks if there are any RT edges connecting to that node. If such an edge is found, it extracts the translation attribute and prints it. Finally, it draws the room polygon and doors using OpenCV.", + "description": "1) retrieves room information from the graph, 2) identifies RT edges connecting to the current room, and 3) extracts translation attribute for the current room.", "params": [ { "name": "room_node_id", - "type_name": "str", - "description": "Used to identify the node representing the room for which the picture is being generated." + "type_name": "str | int", + "description": "Required, indicating that it is mandatory to pass this value for the function to work correctly." } ], "returns": null, + "usage": { + "language": "python", + "code": "# Creating a specific worker object\nspecific_worker = SpecificWorker(proxy_map)\n\n# Generating a room picture for a specific room node id\nspecific_worker.generate_room_picture(123456789)\n\n# Saving the image to a file\ncv2.imwrite(\"room_image.jpg\", specific_worker.room_image)\n", + "description": "" + }, "name": "generate_room_picture", "location": { - "start": 688, - "insert": 690, + "start": 707, + "insert": 709, "offset": " ", "indent": 8, "comment": null @@ -1825,23 +1960,28 @@ "docLength": null }, { - "id": "9ac71498-065b-518a-7348-6e9a0a780cff", + "id": "9d6cc73f-8cb4-e89e-7945-94e9bd98c8ac", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Inserts or assigns an edge with specified attributes to the graph represented by the `self.g` attribute, using the `insert_or_assign_edge` method provided by the Graph class.", + "description": "Inserts or assigns an edge in the graph representing the room hierarchy, with the current worker's ID as the source and the current room ID as the target.", "params": [ { "name": "room_id", "type_name": "str", - "description": "Used to identify the current room of the agent that is performing the action." + "description": "Used as an identifier for the current room that the agent is located in." } ], "returns": null, + "usage": { + "language": "python", + "code": "def insert_current_edge(self, room_id):\n current_edge = Edge(room_id, room_id, \"current\", self.agent_id)\n self.g.insert_or_assign_edge(current_edge)\n", + "description": "\nIn this example, the SpecificWorker class is used to insert an edge into a graph representing a LongTermSpatialMemory with the robot's current location represented by room_id. The function first creates an Edge object with the same room_id as the target and source node ids. Then, it inserts or assigns the created edge into the graph using the insert_or_assign_edge() method of the DSRGraph class." + }, "name": "insert_current_edge", "location": { - "start": 723, - "insert": 725, + "start": 742, + "insert": 744, "offset": " ", "indent": 8, "comment": null @@ -1851,28 +1991,33 @@ "docLength": null }, { - "id": "19662af0-764b-af81-c34a-5d2119ef52c6", + "id": "339e852e-5da6-aa92-6146-a2388fe634c4", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Updates a node's properties based on its type and other considerations, such as checking if a door node has a pre-transition edge, inserting a new vertex or edge in the graph if necessary, and drawing the graph.", + "description": "Updates a node in the graph based on its ID and type. If the type is \"door\", it inserts a vertex in the long-term graph, checks if a room node exists, and inserts an edge with a specific translation and rotation. If the ID is the affordance node active ID, it checks the state and activeness of the node and transitions to the crossed state.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to identify the node to be updated." + "description": "Used to identify the node being updated." }, { "name": "type", "type_name": "str", - "description": "Used to determine the action taken on the node based on its name." + "description": "Used to identify the node being updated, with possible values of \"door\" or \"room\"." } ], "returns": null, + "usage": { + "language": "python", + "code": "# Create a specific worker object and initialize it with the startup check flag set to True\nspecific_worker = SpecificWorker(proxy_map, startup_check=True)\n\n# Call the update_node method of the specific worker object with the node ID and type as arguments\nspecific_worker.update_node(1234567890, \"door\")\n", + "description": "\nThis code creates a SpecificWorker object and initializes it with the startup check flag set to True. Then it calls the update_node method of the specific worker object with the node ID 1234567890 and type \"door\" as arguments. The function would then check if there is an igraph vertex for the door node, insert it if necessary, and then check if there are any edges between the parent node and the door node in the long-term graph, inserting them if necessary and drawing the updated graph." + }, "name": "update_node", "location": { - "start": 737, - "insert": 738, + "start": 756, + "insert": 757, "offset": " ", "indent": 8, "comment": null @@ -1882,33 +2027,921 @@ "docLength": null }, { - "id": "dadeb4c6-dedc-1490-c447-209f1bc1d73a", + "id": "2abe3cea-a1da-b5bc-224c-fb099b8b8257", "ancestors": [ - "d14b1fa8-9d9a-61a7-184c-3f4e98189154" + "8012c732-b5dd-93a9-2441-d47f31f3d2fa" ], - "description": "Updates the current edge of a room node based on certain conditions. If the target node is the robot id, and there are no edges of type \"current\" in the graph, the function inserts the current edge and sets it as current.", + "description": "Updates the current edge based on the specified frame, to node, and type. If no current edge exists, it inserts a new edge and sets it as the current one.", "params": [ { "name": "fr", "type_name": "int", - "description": "Referred to as \"from room\" in the code snippet provided." + "description": "Representative of the 'from' node value, which indicates the starting point of an edge" }, { "name": "to", "type_name": "int", - "description": "A reference to an integer representing a node ID in the graph." + "description": "The target node ID of an edge to be updated." }, { "name": "type", "type_name": "str", - "description": "Used to indicate the type of edge being updated, specifically \"RT\"." + "description": "Used to specify the type of edge being updated, either \"RT\" for real-time or \"NRT\" for non-real-time." } ], "returns": null, + "usage": { + "language": "python", + "code": "worker = SpecificWorker(proxy_map)\n# Initialize the worker with the given proxy map and startup check set to False.\n\nworker.update_edge(fr=10, to=20, type=\"RT\")\n# Update the edge connecting nodes 10 and 20 with type \"RT\".\n", + "description": "" + }, + "name": "update_edge", + "location": { + "start": 811, + "insert": 815, + "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": 792, - "insert": 796, + "start": 806, + "insert": 810, "offset": " ", "indent": 8, "comment": null diff --git a/.komment/komment.json b/.komment/komment.json index 7052d64c..fde23142 100644 --- a/.komment/komment.json +++ b/.komment/komment.json @@ -1,7 +1,7 @@ { "meta": { "version": "1", - "updated_at": "2024-07-12T12:07:42.963Z", + "updated_at": "2024-07-15T13:16:52.559Z", "created_at": "2024-07-03T15:48:27.349Z", "pipelines": [ "50c96b7c-ad3b-4b93-ae19-7f6fbd5637f7", @@ -9,7 +9,8 @@ "510e9758-a82c-441e-9653-dc071bb409eb", "1f636eb0-7f1e-4d1d-b462-6d1eff3533cb", "d6a72e5c-096b-4179-ac39-3343ea5ec3a5", - "395b32ba-01b1-45bb-84e0-96a1b0abb44e" + "395b32ba-01b1-45bb-84e0-96a1b0abb44e", + "fb0c09b8-650c-42fb-a814-0c07d121ec67" ] }, "lookup": [ @@ -20,7 +21,8 @@ "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.py", + "agents/long_term_spatial_memory_agent/src/specificworker_sec.py" ] ] } \ No newline at end of file diff --git a/agents/g2o_agent/src/specificworker.py b/agents/g2o_agent/src/specificworker.py index c10e653c..ad616c9f 100644 --- a/agents/g2o_agent/src/specificworker.py +++ b/agents/g2o_agent/src/specificworker.py @@ -49,130 +49,129 @@ class SpecificWorker(GenericWorker): """ - Manages a graph, updates node attributes, and sets edges based on robot odometry - data and room changes. It also handles RT edge tracking and stores translation - and rotation values for later use. + Manages a specific robot's state and updates its graph, edges, and attributes + based on messages from the occupancy grid. It also handles room changes and + RT sets for the specified robot. Attributes: - Period (instance): Used to control the update rate of the graph. It sets - the time interval between successive updates in milliseconds, which - can be used to optimize the performance of the worker. - agent_id (int): Used to identify the agent that owns the robot. It is used - to determine which agent's graph should be updated when a new edge is - added or an existing edge is modified. - g (Graph): Used for representing the robot's environment through a graph, - with nodes representing rooms and edges representing the movement of - the robot between them. - startup_check (QTimersingleShot): Used to check if the user wants to quit - the application after a certain period of time has passed since the - last update. It schedules a single shot event every 200 milliseconds - to check if the user wants to quit. - rt_api (instance): Used to store information related to the RT (Real-time) - module, such as the last time it was accessed and the translation and - rotation values set for the current room. - inner_api (instance): A method that updates the graph with new nodes, - edges, or attributes based on the inner loop of the worker. - odometry_node_id (int): Used to store the ID of the node representing the - robot's position in the graph. - odometry_queue (list): Used to store the current odometry data of the - robot, including its advance speed, side speed, and angular speed, - which are updated at a rate of 200 Hz. - last_odometry (int): Used to store the time at which the worker last - received odometry data from the environment. It is updated every time - a new odometry message is received, allowing the worker to determine - how long ago it received the message. - g2o (Graph): Used to store the graph data of the environment. It contains - the nodes, edges, and attributes of the graph. - odometry_noise_std_dev (floatingpoint): Used to specify the standard - deviation of noise added to the odometry measurements for training purposes. - odometry_noise_angle_std_dev (float): 0.1 by default, representing the - standard deviation of the noise added to the robot's angle readings - during odometry estimation. - measurement_noise_std_dev (float): 0.1 by default, representing the standard - deviation of measurement noise for the robot's sensors. It determines - how much noise is added to the measured positions and orientations of - the robot in the simulation. - last_room_id (int): Used to store the last room ID seen by the worker - before changing rooms. - actual_room_id (int): Used to store the current room ID of the agent during - navigation. - elapsed (int): Used to keep track of the time elapsed since the last call - to the `startup_check()` function, which is used to check if the - application should be closed after a certain period of inactivity. - room_initialized (bool): Used to track whether the current room has been - initialized or not, it's set to False when a new room is entered and - True otherwise. - iterations (int): 0-indexed, indicating the number of iterations of the - worker's tasks it can perform before finishing. - hide (str): Used to determine whether a node or edge should be hidden from - the graph. It allows you to specify which nodes or edges to hide based - on their ID, label, or other attributes. - init_graph (instance): Used to keep track of whether the graph has been - initialized or not, it's set to `True` when the graph is first created - and `False` otherwise. - current_edge_set (bool): Used to track whether the current edge being - processed is part of the RT set or not. - first_rt_set (bool): Set to True when a new RT (remote transmission) set - is encountered for the first time during the simulation, indicating - that the worker has entered a new room or area. - translation_to_set (3D): Used to store the translation of a robot's end - effector when it enters a specific set. It is used in conjunction with - the - `rotation_to_set` attribute to determine the full pose of the robot - when it enters the set. - rotation_to_set (3D): Used to store the rotation of the robot relative to - its set position. - room_polygon (QPolygon): Used to store the polygon representation of a - room in the environment. - security_polygon (QPolygon): Used to store the security polygon of a - specific worker in a graph, which is used for collision detection and - response in robotics. - initialize_g2o_graph (instance): Used to create a Graph2O graph from a - robot's observation, which is then used for optimization. It initializes - the graph by adding nodes and edges based on the robot's observation - and sets up the necessary attributes for optimization. - rt_set_last_time (int): Used to track the time since the last RT set was - received for a given agent ID. It is used in the `update_edge()` - function to determine if enough time has passed since the last RT set - for the robot to consider setting a new translation and rotation. - rt_time_min (float): Set to the minimum time interval between two RT sets - that are considered as a new RT set. It is used to determine when to - reset the translation and rotation to set values in the `update_edge()` - method. - last_update_with_corners (int): Used to keep track of when the worker last - updated its graph with corners. It is set to the current time whenever - the `update_node`, `update_edge`, or `delete_edge` methods are called, - and is used to determine when the graph has changed significantly - enough to warrant updating the robot's state. - timer (QTimer): Used to schedule a call to the `QApplication.instance().quit()` - function after 200 milliseconds, which means that the worker will quit - after a certain amount of time has passed. - compute (instance): Used to handle updates for node attributes. It takes - two arguments: `id` which is the id of the node to be updated, and - `attribute_names` which is a list of strings representing the names - of the attributes to be updated. - update_node_att (event): Called when a new node attribution is received - from the graph. It updates the robot's odometry queue with the current - advance, side, and angular speed and sets the `room_initialized` flag - to false if the room ID changes. - update_edge (update_edge): Used to update the attributes of an edge in a - graph based on its type, such as "current" or "RT". It sets the - translation and rotation of the robot based on the RT set. - update_edge_att (str): Defined as a method that updates the attributes of - an edge in the graph based on a specific edge type and a list of - attribute names. + Period (float|int): 200 by default, indicating the time interval (in + milliseconds) between updates of the graph. It can be modified to + adjust the update rate of the worker. + agent_id (int|str): A unique identifier for the agent, which can be used + to identify specific agents within the system. + g (Graph|QGraphicsScene): Used to store and manipulate the graph data structure. + startup_check (QTimersingleShot200,QApplicationinstancequit): Set to call + the quit function of the QApplication instance after a delay of 200 milliseconds. + rt_api (Union[int,str]): Used to store the API for the Robot Translation + (RT) functionality in the worker. It can take the value of either an + integer representing the RT algorithm or a string representing the RT + algorithm name. + inner_api (Dict[str,Any]): Used to store additional internal data for the + worker, such as the current room ID or the first RT set. It is only + accessible within the worker's implementation and is not part of the + public API. + odometry_node_id (int): Used to identify the node in the graph that + represents the robot's position and orientation. + odometry_queue (List[Tuple[float,float,float,int]]): Used to store the + latest odometry data from the robot's sensors for graph updating. + last_odometry (float|List[float]): Used to store the last known odometry + information of the robot, including its position, velocity, and angular + velocity, which are updated every 200 milliseconds. + g2o (Graph|npndarray): Used to represent the graph of the environment, + allowing the worker to perform operations on it such as adding or + deleting nodes and edges. + odometry_noise_std_dev (float|int): Used to represent the standard deviation + of noise in the odometry measurements. It determines how much the + actual robot position, orientation, and velocity are expected to deviate + from their predicted values based on the odometry measurements. + odometry_noise_angle_std_dev (float|int): 1.0 by default, which represents + the standard deviation of the noise angle in the odometry measurement. + It helps to quantify the uncertainty in the estimated angle of the + robot's movement. + measurement_noise_std_dev (float|int): Used to represent the standard + deviation of measurement noise in the robot's odometry readings. It + is used to estimate the uncertainty of the robot's position and velocity. + last_room_id (str|int): Used to store the last room ID seen by the agent + before changing rooms, used for updating the RT set. + actual_room_id (str|int): Used to store the current room ID of the agent, + which is updated when the agent moves to a new room. + elapsed (float|int): Used to keep track of the time since the last call + to the `update()` method, which is used to control the frequency of + updates to the graph. + room_initialized (bool): Set to False when a room change is detected, + indicating that the worker has switched rooms. It is then reset to + True once the new room's ID is determined. + iterations (int|List[int]): Used to keep track of the number of iterations + of the worker's function that have been performed, or the list of + iteration numbers if the function is called multiple times with different + inputs. + hide (bool): Used to hide the worker from the graphical user interface + (GUI) when set to True. + init_graph (bool): Used to track whether the graph has been initialized + by the worker during its lifetime. It is used to prevent unnecessary + work from being performed when the graph has already been initialized. + current_edge_set (bool): Used to indicate whether the current edge being + processed is part of a RT set or not. It is set to True when a new RT + edge is encountered, and False otherwise. + first_rt_set (bool): Set to True when the robot performs its first RT + (Real-time) movement, indicating that the worker has started tracking + RT movements. + translation_to_set (float|List[float]): Used to store the translation value + to set for the shadow robot. The list stores the values of the translation + in the x, y, and z dimensions respectively. + rotation_to_set (float|int): Used to store the rotation of the robot to + set its position relative to its starting point. + room_polygon (Tuple[float,float,float,]): Used to store the coordinates + of a room's polygon in a graph. + security_polygon (Polygon|List[Point]): Used to store the security polygon + of a room, which is used for collision detection and avoidance during + robot navigation. + initialize_g2o_graph (void|QTimersingleShot200,QApplicationinstancequit): + Used to initialize a Graph2O graph for representing the environment. + rt_set_last_time (int|float): Used to track the time since the last RT + (Room To) set by the agent. It is used to determine when to reset the + translation and rotation to set values. + rt_time_min (float|int): Defined as a minimum time gap between two successive + RT edge sets, used to determine when to update the translation and + rotation values for the agent. + last_update_with_corners (int|bool): Used to keep track of when the worker + has last updated its state with corners data. It is initially set to + True, indicating that the worker has not yet received any corners data, + and then is updated to False whenever the worker receives new corners + data. + timer (int|float): Used to track the time elapsed since the last update + of the graph, with the purpose of checking if it's been a certain + amount of time (200ms) since the last update, and if so, quit the application. + compute (Callable[[float,float],float]): Used to compute the next node ID + for the worker's node. It takes two arguments: the current time and a + seed value, and returns the next node ID as a float value. + update_node_att (Tuple[int,str,int,int]): Used to update the attributes + of a node in the graph when the node's ID matches the given ID. The + attribute takes four arguments: the node ID, the attribute names, the + old value, and the new value. + update_edge (Callable[float,float,str]): Used to update the edge attributes + of a graph based on specific conditions. It takes three arguments: the + first is the source node ID, the second is the target node ID, and the + third is the edge type. The attribute is called whenever a new edge + is added or an existing edge is modified in the graph, and it can be + used to set or update edge attributes based on specific conditions. + update_edge_att (List[str]): Used to update the attributes of an edge based + on a specific type and name. It takes three parameters: the first is + the id of the edge, the second is the type of edge, and the third is + a list of attribute names to be updated. """ def __init__(self, proxy_map, startup_check=False): """ - Initializes a SpecificWorker object, setting its properties and connecting - to signals for updating node attributes, edges, and edge attributes. + Initializes the worker's internal state, including its graph, odometry + queue, and other components essential for its operation. Args: - proxy_map (dict): Used to specify a mapping from real-world coordinates - to virtual coordinates for the robot's movement. - startup_check (bool): Used to run an initialization check on the graph - when the agent starts up. + proxy_map (Dict[str, Any]): Used to pass additional data to the + SpecificWorker object. + startup_check (bool): Used to check if the graph has been properly + initialized before starting the worker's computation. If set to + `False`, it will skip this check and proceed with the computation. """ super(SpecificWorker, self).__init__(proxy_map) @@ -249,9 +248,8 @@ def setParams(self, params): @QtCore.Slot() def compute(self): """ - In the `SpecificWorker` class implements a robot's movement using ROS - navigation stack, computing the robot's position and orientation based on - the previous odometry data and adding it to the graph. + Processes robot odometry data and updates the graph using G2O optimization + to estimate the robot's position and orientation. """ if time.time() - self.elapsed > 1: @@ -321,13 +319,18 @@ def compute(self): # Iterate over door nodes if self.security_polygon.containsPoint(robot_point, Qt.OddEvenFill): for door_node in door_nodes: - 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]) + 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) @@ -380,13 +383,13 @@ def initialize_g2o_graph(self): # print("Initializing g2o graph") # get robot pose in room """ - Initializes a Graph2Online (G2O) graph for a specific robot's odometry - data, by adding nominal corners and fixed poses to the graph based on room - and door nodes in the environment map. + 1) initializes the g2o graph based on room and robot nodes, 2) adds nominal + corners for rooms and doors, and 3) fixes the pose of a robot node using + odometry information. Returns: - bool: 1 if the function was able to initialize the g2o graph successfully, - and 0 otherwise. + bool: 1 if the g2o graph is successfully initialized with nominal + corners and fixed poses, and 0 otherwise. """ self.g2o.clear_graph() @@ -593,17 +596,18 @@ def initialize_g2o_graph(self): def get_displacement(self, odometry): """ - Calculates the displacement of an agent based on its odometry data, taking - into account advancement, lateral movement, and angular movement. + Calculates the displacement of the robot in three dimensions (lateral, + forward, and angular) based on the odometry data stored in a queue. Args: - odometry (dict): Passed as an argument to the function, containing the - odometric data of the vehicle at each time step, including the - position, velocity, and timestamp. + odometry (Tuple[float, float, float]): A sequence of 3-element tuples + representing the robot's position, velocity, and timestamp in a + time interval. Returns: - 3element: 3-D vector representing the displacement of a robot's end - effector in terms of lateral, forward, and angular displacements. + Tuple[float,float,float]: 3 floating-point values representing the + lateral displacement, forward displacement, and angular displacement + of an object over a given time interval. """ desplazamiento_avance = 0 @@ -626,16 +630,18 @@ def get_displacement(self, odometry): def get_covariance_matrix(self, vertex): """ - Computes the covariance matrix of a set of vertices in a graph, using the - gradient descent optimizer from the `g2o` library. It returns the computed - covariance matrix and whether it was successfully computed or not. + Computes the covariance matrix between a given vertex and all other vertices + in the graph, using the G2O optimizer to compute marginals and upper + triangle matrix representation. Args: - vertex (g2overtex): Used to represent a specific vertex in a graph. + vertex (G2O.Vertex | G2O.HessianIndex): Used to compute the covariance + matrix for a specific vertex in the graph. Returns: - tuple: 2-element tuple containing two values: (`covariances_result`, - `covariances`). + numpyndarray: 2-dimensional and upper-triangular matrix representing + the covariance matrix between a given vertex and all other vertices + in the graph. """ cov_vertices = [(vertex.hessian_index(), vertex.hessian_index())] @@ -652,13 +658,12 @@ def get_covariance_matrix(self, vertex): def visualize_g2o_realtime(self, optimizer): """ - Visualizes the 3D positions of vertices and edges in a G2O file in real-time, - using matplotlib. It loads the G2O file, calculates the positions of the - vertices and edges, and plots them on a 3D scatter plot. + Visualizes the real-time optimization process of a G2O algorithm by plotting + the positions and edges of the vertices in 3D space. Args: - optimizer (instance): Used to store an instance of the G2O optimizer - class, which is responsible for minimizing the energy of the system. + optimizer (object | Optimizer): Used to load G2O files, estimate vertex + positions, and visualize them in real-time. """ plt.ion() @@ -707,14 +712,13 @@ def update_node_att(self, id: int, attribute_names: [str]): # print("INIT GRAPH") """ - Updates the attributes of a specific node in a graph, based on the current - time and other factors. + Updates the attributes of a node in a graph, specifically the odometry queue. Args: - id (int): Used to represent the unique identifier of the node being - updated, specifically the odometry node ID. - attribute_names ([str]): An array of names of attributes to be updated - on the node. + id (int): Represented as an integer value that identifies the node to + be updated. + attribute_names ([str]): An array containing the names of attributes + to be updated for the given node ID. """ if id == self.odometry_node_id: @@ -735,24 +739,25 @@ def update_node(self, id: int, type: str): # self.room_initialized = True if self.initialize_g2o_graph() else False # self.init_graph = True """ - Updates an unspecified node with an ID and a type. If the type is "corner", - it initializes a room graph if necessary. Otherwise, it does nothing. + Updates a node with ID `id`. The method checks if the node type is "corner", + and if so, it retrieves the room node from the graph and sets a flag + indicating that the room has been initialized. Args: - id (int): Used to represent the unique identifier for the node being - updated. - type (str): Defined as "corner". + id (int): Used to identify the node to be updated. + type (str): Used to specify the node's type. """ pass def delete_node(self, id: int): """ - Deletes a node from a data structure managed by a `SpecificWorker` subclass - of `GenericWorker`. + Deletes a node from a list or dict based on its ID, setting `self.room_initialized` + to `False`. Args: - id (int): Used to identify the node to be deleted. + id (int): Intended to represent the unique identifier for the node + being deleted. """ pass @@ -765,15 +770,15 @@ def delete_node(self, id: int): def update_edge(self, fr: int, to: int, type: str): """ - Updates the room ID and sets the `current_edge_set` attribute based on the - type of edge received, and also performs RT translation and rotation if necessary. + Updates the room ID and sets the current edge set based on the type of + edge received from the graph. Args: - fr (int): Representative of the starting vertex of an edge in a graph. - to (int): Used to represent the ID of the next node in the graph that - the edge will be attached to. - type (str): Used to indicate the type of edge being updated, either - "current" or "RT". + fr (int): Representing the starting node index of an edge in a graph. + to (int): The target node index of the edge to be updated, which can + be either a room or the Shadow node. + type (str): Used to specify the edge type, which can be either "current" + or "RT". """ if type == "current" and self.g.get_node(fr).type == "room": @@ -805,13 +810,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): """ - Deletes an edge from a graph, specified by its ID and type. + Deletes an edge from a graph, specified by its index (fr) and the index + of its adjacent vertex (to). The edge's type can be specified for further + filtering. Args: - fr (int): Passed as an argument to the function with the value of 123. - to (int): Used to specify the target vertex ID for edge deletion. - type (str): Used to specify the edge type to be deleted, which can be - either 'weighted' or 'unweighted'. + fr (int): 1st argument or input of the function indicating the first + vertex or node to delete an edge from. + to (int): Used to specify the destination vertex ID for the edge + deletion operation. + type (str): Used to indicate the edge type to be deleted, with possible + values 'in' or 'out'. """ pass 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 d221b6c4..a6b4663f 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 @@ -14,34 +14,33 @@ class LongTermGraph: """ - Draws a long-term graph representation of a building's layout and room usage, - allowing for interactive exploration and analysis of the data. It provides - methods to draw the graph, add points, and display room names and door connections. + Acts as a plotter for a graph representation of a metric reconstruction. It + provides methods to draw rooms and doors, and to customize the plot's appearance. Attributes: - g (instance): Used to specify the color scheme for the graph, where `'r'` - represents red, `'b'` represents blue, and `-` represents gray. - read_graph (instance): Used to read a graph from a file specified by the - user. - fig (matplotlibfigureFigure): Used to store the figure object that represents - the graph. - ax (matplotlibaxesAxes): Used to interact with a specific axis object in - a figure. It provides methods for adding plots, annotations, and other - visual elements to the axis. - fig_2 (matplotlibfigureFigure): Used to store the figure object for the - metric reconstruction plot. - ax_2 (matplotlibfigureFigure): Used to represent the second plot, which - shows the metric reconstruction. + g (str|int): Used to specify the color of the graph's edges, defaulting + to 'r-' for non-current rooms and 'g-' for current rooms. + read_graph (Callable[[Union[str,Path],int],Graph]): Used to read a graph + from a file specified by the file path or name. + fig (matplotlibfigureFigure): Used to represent the figure instance for + visualization of the graph. + ax (matplotlibpyplotAxes): Used to represent the axis of the graph. It + provides methods for setting various properties of the axes, such as + titles, labels, limits, and annotations. + fig_2 (matplotlibfigureFigure): Used to store the second figure object + that is created for visualizing the metric reconstruction. + ax_2 (matplotlibpyplotAxes): Used to access and manipulate the second axes + object created by the `subplot()` function in the parent plot. """ def __init__(self, file_name): """ - Reads a graph from a file, creates two subplots for visualization, and - sets up the x-axis and y-axis labels. + Reads a graph from a file and creates an instance of the Graph class, + optionally creating a figure and axes to visualize the graph. Args: - file_name (str): Used to specify the path to a GraphML file from which - the graph structure is read. + file_name (str): Used to specify the path to a graph file from which + the graph will be read. """ try: @@ -49,7 +48,7 @@ def __init__(self, file_name): 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() @@ -95,7 +94,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: @@ -140,10 +140,10 @@ 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") - + print("Corners transformed", corners_transf) 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] - + print("Corners in room", corners_in_room) 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)] @@ -282,13 +282,12 @@ def check_point_in_map(self, rooms_map: dict, point: QPoint): def draw_graph(self, only_rooms=True): """ - Generates a graph representation of a long-term state machine (LTSM) based - on its adjacency matrix. It creates the graph layout, defines edge colors, - and adds node and edge labels. + Creates a graph based on a directed graph represented by a NetworkX graph + object, and displays it using Matplotlib's scatter and plot functions. Args: - only_rooms (bool): Used to filter nodes based on their type. It restricts - the subgraph to only include nodes labeled as "room". + only_rooms (bool): Used to filter the nodes and edges of the graph + according to their types, only including rooms and doors. """ self.ax_2.clear() diff --git a/agents/long_term_spatial_memory_agent/src/specificworker.py b/agents/long_term_spatial_memory_agent/src/specificworker.py index 91560f33..a86380c0 100644 --- a/agents/long_term_spatial_memory_agent/src/specificworker.py +++ b/agents/long_term_spatial_memory_agent/src/specificworker.py @@ -48,84 +48,81 @@ class SpecificWorker(GenericWorker): """ - Manages a graph representing a robot's movement and interactions with its - environment, performing various operations such as inserting, updating, and - deleting nodes and edges, as well as tracking the robot's state. + Manages a graph representing a manipulation task, providing methods for + inserting, updating, and deleting nodes and edges. It also keeps track of the + robot's current state and performs actions based on that state. Attributes: - Period (str): Used to store the current time period in which the worker - is active, allowing for more efficient handling of tasks during different - time slots. - agent_id (int): Used as the ID of the agent that owns or manipulates the - graph, which could be a robot or a human operator. - g (igraphGraph): Used for manipulating the graph in various methods like - `insert_igraph_vertex`, `insert_igraph_edge`, etc. - update_node (int): Used to update a node's attributes based on its ID. It - takes two arguments: `id` (int) and `type` (str), where `id` is the - node's ID to be updated and `type` represents the type of update (e.g., - "door", "room"). - update_edge (int): Used to update the edge of a robot moving from one node - to another with a certain type (current) when there is no current edge - and a room node exists. - startup_check (str): Used to check if the worker is in startup mode or - not. It is set to "true" when the worker starts up and "false" otherwise. - rt_api (str): Represented as a string value indicating which RT (Real-Time) - affordance to use. - inner_api (instance): A Python method that returns the inner API of the - worker. It is used to define the functionality of the worker and its - interactions with other components of the system. - robot_name (str): Used to store the name of the robot being controlled by - the worker. - robot_id (int): Used to identify the robot node in the graph. - last_robot_pose (IGraph): Used to store the last known pose of the robot, - which can be used for debugging purposes or to track the robot's movement. - robot_exit_pose (RT): A dictionary containing the pose (position and - orientation) of the robot when it exits the room through the door. - state (str): Used to store the current state of the worker, which can be - either "idle", "working", or "crossed". - affordance_node_active_id (int): Used to store the ID of the affordance - node that is currently active. It is used to check if the affordance - node is completed and not active, and to switch to the crossed state - when it is completed and not active. - exit_door_id (int): 1234, which represents the ID of the door node that - serves as the exit of a room. - room_exit_door_id (int): Used to represent the ID of the door node that - leads from a room to the outside world in the graph. - enter_room_node_id (int): 0 by default, representing the ID of the room - node that the worker enters when it completes its task. - graph (igraphGraph): Used for storing and manipulating the graph represented - by the worker. It contains the nodes, edges, and other attributes of - the graph. - vertex_size (int): Used to store the size of the vertex (node) in the - graph, indicating the number of attributes associated with each node. - not_required_attrs (list): Used to keep track of a list of attributes that - are not required for the worker's functionality, but can be useful for - debugging or other purposes. - long_term_graph (igraphGraph): Used for storing the long-term graph of the - environment, which is updated when certain events occur like inserting - or deleting nodes. - global_map (igraphGraph): Used to store the global map of the environment, - which is a graph representation of the environment that includes all - nodes and edges. - insert_current_edge (igraphEdge): Used to insert a new edge into the graph - with the given source and target nodes, and the specified edge type. - timer (int): Set to the number of milliseconds since the epoch (January - 1, 1970, 00:00:00 UTC) when the worker was created. It is used to track - the elapsed time for the worker's tasks. - compute (instance): Used to compute the shortest path between two nodes - in the graph based on the RT model. + Period (str|int): Used to store the period of time during which the worker + performs its task, either in seconds or minutes, depending on the value + assigned to it. + agent_id (int|str): Used to store the ID of the specific worker for which + the methods of this class are intended to work. + g (igraphGraph): Used for handling long-term dependencies between nodes + in the graph. + startup_check (bool|str): Used to check if the worker has started successfully + or not. It can be set to a specific value like "success" or "failure" + to indicate whether the startup was successful or not, or it can be + left blank to default to a boolean value indicating whether the worker + started successfully or not. + rt_api (str|int): Used to store the RT (Real-Time) API of the worker, which + allows the robot to perform real-time actions. + inner_api (Dict[str,Any]): Used to store internal APIs for the worker's + methods that are not exposed to the outside world. + robot_name (str|List[str]): Used to store the name of the robot that the + worker belongs to. + robot_id (int): Used to represent the unique identifier of the robot node + in the graph. + last_robot_pose (Dict[str,float]): Used to store the last known pose of + the robot before it entered a room or crossed a boundary. + robot_exit_pose (str|int): Used to store the ID of the room node that the + robot exits through when transitioning from a door node to a room node. + state (str|bool): Used to keep track of the worker's current state (either + "idle", "crossed", or "completed"). + affordance_node_active_id (int|bool): Used to store the ID of the affordance + node that is currently active or not. + exit_door_id (int|str): Used to store the ID of the door node that serves + as an exit from a room. + room_exit_door_id (int|str): 16 by default, representing the id of a + specific door node in the long-term graph that marks the exit of a room. + enter_room_node_id (int|str): Represented as a room number that, when + updated to a value greater than zero, indicates that the robot has + entered a new room. + vertex_size (int|str): Used to store the size or dimension of a vertex in + the graph, which can be used to determine the size of a node in the graph. + not_required_attrs (List[str]): Used to store a list of attributes that + are not required for the worker's operations. It helps to optimize the + worker's performance by ignoring these unused attributes during updates, + inserts, or deletes. + long_term_graph (igraphGraph): Used to store the long-term graph representation + of the environment, which is updated during the worker's execution. + insert_current_edge (NoneOrEdge|List[str]): Used to insert a new edge into + the graph with the specified fr and to nodes, and sets it as the current + edge if no current edge exists. + timer (int|float): Used to track the time taken by the worker to process + a task. + compute (Callable[[str,str],Any]): Used to specify a function that computes + the next action for the robot based on its current state and the environment. + update_node (Dict[str,Any]): Used to update a node in the graph based on + its ID, type (door or room), and name. It performs various actions + such as checking if a door node exists, inserting a door node, updating + the RT translation and rotation, and drawing the graph. + update_edge (Union[str,int]): Used to update the edge attributes of a + specific robot node in the graph when the node's door is opened. """ def __init__(self, proxy_map, startup_check=False): """ - Initializes an instance of the SpecificWorker class, setting up graph - connections and variables for storing information about a robot's environment - and state. + Initializes instance variables and sets up event listeners for graph + updates, timeouts, and node measurements. Args: - proxy_map (igraphGraph): Used to represent the graph mapping between - nodes and objects, allowing for efficient object manipulation. - startup_check (bool): Used to check if the agent has already been started. + proxy_map (Dict[str, Any]): Used to pass additional data to the + SpecificWorker object. It contains key-value pairs that can be + used to store any information required by the worker. + startup_check (bool): Used to check if the agent has already started + its computation, skipping the computation if it has already been + done. """ super(SpecificWorker, self).__init__(proxy_map) @@ -135,17 +132,6 @@ 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: @@ -171,13 +157,21 @@ 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"] # 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) + + + # In case the room node exists but the current edge is not set, set it room_nodes = self.g.get_nodes_by_type("room") @@ -190,21 +184,32 @@ def __init__(self, proxy_map, startup_check=False): 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 the parameters of an object, then removes a self-edge from a room and - stores the ID of an exit door in a variable. It also assigns attributes - to both doors in the new room. + Sets the parameters passed as an argument to `True`. Then, it removes a + self-edge from the current room, stores the ID of the exit door in a + variable, and sets the name attribute of both doors to the other side + door's name. Args: - params (object): Used to store any relevant data or configuration for - the function's operation. + params (bool): Passed to set parameters for an object of class `Room`. Returns: - Boolean: True. + bool: True. """ return True @@ -222,29 +227,63 @@ 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) - + # # Check if graph exists """ - Determines the current state of a worker and performs the appropriate - actions based on that state, including idle, crossing, crossed, initializing - room, known room, initializing doors, storing graph, and removing. + Computes and updates the robot's RT (reactive transport) based on the + current state of the graph. """ + 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) + print("1") + + 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) + + + match self.state: case "idle": self.idle() @@ -267,11 +306,9 @@ def compute(self): 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 """ - Performs the following tasks: - 1/ Identifies active affordance nodes in the short-term graph. - 2/ Checks if any "current" edge exists in the short-term graph. - 3/ If a door is found, computes its pose in the long-term graph and - associates it with the current room. + Computes the affordance pose of the robot in a new room based on its current + position and orientation, and associates doors in the long-term graph with + their corresponding rooms. """ aff_cross_nodes = [node for node in self.g.get_nodes_by_type("affordance") if node.attrs["active"].value == True] @@ -289,8 +326,6 @@ def idle(self): 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 @@ -376,9 +411,8 @@ def idle(self): def crossed(self): # Get parent node of affordance node """ - Determines the current room based on an active affordance node and updates - the state machine accordingly. If the node has no parent, it sets the exit - door ID to the value of the active affordance node's parent attribute. + Updates the state of the worker based on the active affordance node and + exit door ID. """ affordance_node = self.g.get_node(self.affordance_node_active_id) @@ -403,8 +437,8 @@ def initializing_room(self): # Get room nodes """ - 1) identifies room nodes in the graph, 2) sets the `enter_room_node_id` - field, and 3) enters the "initializing doors" state. + Initializes the room nodes and sets the enter room node ID, then transitions + to the "initializing doors" state. """ 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] @@ -421,18 +455,18 @@ def initializing_room(self): def known_room(self): # Get other side door name attribute """ - Within the `SpecificWorker` class defines how a robot navigates through a - graph representation of a maze to reach an objective room while avoiding - obstacles and taking into account the associated door's rotation. + Determines the room name associated with a door based on its ID and updates + the graph with new edges and nodes to simulate robot movement through the + door and into the adjacent room. """ 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 + # 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: - # 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) + # 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 @@ -471,7 +505,7 @@ def known_room(self): else: print("Going out door", new_door_name) try: - new_door_node = self.graph.vs.find(name=new_door_name) + 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 @@ -521,8 +555,9 @@ def known_room(self): def initializing_doors(self): # Check if node called "room_entry" of type room exists """ - Identifies doors connected to a specified exit door and associates them - with each other, updating node attributes and the state of the worker. + 1) retrieves exit edges and matching doors, 2) sets other side door name + and connected room name attributes for each exit door node, and 3) associates + doors using their names and connected rooms. """ exit_edges = [edge for edge in self.g.get_edges_by_type("exit") if edge.destination == self.exit_door_id] @@ -565,28 +600,27 @@ def initializing_doors(self): 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. + Connects two doors in a graph by adding an edge between their nodes and + updates their attributes to reflect the connection. Args: - door_1 (str): A string representing the name of the first door to be - associated with another door in the graph. - door_2 (str): Represented as an igraph vertex object, which contains - information about the door node to be associated with the first - door node passed as input. + door_1 (str): A list containing the name of the first door to be + associated with the long-term graph. + door_2 (str): A name of another door in the graph, which will be + connected to the current door node through an edge in the graph. """ 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]) 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]) 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] @@ -595,14 +629,14 @@ def associate_doors(self, door_1, door_2): def store_graph(self): """ - Within SpecificWorker, a subclass of GenericWorker, stores a graph - representation of a room and its exits in a file called "graph.pkl". + Saves the graph representation of a room to a file named "graph.pkl" using + Python's pickle module. """ 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") @@ -610,16 +644,13 @@ 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 """ - Removes edges from the graph that connect nodes representing rooms, based - on their room numbers. It first identifies edges with room numbers matching - those of the current room, then deletes them and updates a dictionary for - future reference. Finally, it draws the updated graph and sets the state - to "idle". + Removes edges from the long-term graph based on room numbers and updates + the state of the worker. """ room_number = self.g.get_node(self.room_exit_door_id).attrs["room_id"].value @@ -649,12 +680,13 @@ def removing(self): def traverse_graph(self, node_id): # Mark the current node as visited and print it """ - Traverses a graph, starting from a given node, and performs a specific - operation (in this case, inserting vertices and edges) on the nodes and - edges it encounters. + Traverses a graph by starting at a designated node and following RT edges + to reach other nodes, inserting vertices and edges into an IGraph object + as it goes. Args: - node_id (int): Used to identify the node that the function operates on. + node_id (int): Used to represent the unique identifier for a node in + the graph. """ node = self.g.get_node(node_id) @@ -667,17 +699,19 @@ def traverse_graph(self, node_id): def traverse_igraph(self, node): """ - Traverses the graph, starting from a given node, and inserts vertices and - edges into a DSR graph based on certain conditions. + Traverses the graph by visiting all vertices that have successors in the + long-term graph, and for each such vertex, it checks if its successor has + a higher level than the current vertex, and if so, it inserts a DSR vertex + and edge between them, and recursively traverses the successor. Args: - node (igraphVertex): Represented by an index in the graph. + node (igraph.Vertex): Used to represent the current node being traversed. """ - vertex_successors = self.graph.successors(node.index) + 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 @@ -688,30 +722,28 @@ def traverse_igraph(self, node): def insert_igraph_vertex(self, node): """ - Adds a vertex to an igraph graph and populates its attributes based on the - given node's attributes. It also attempts to find an edge connecting the - new vertex to another vertex with a matching "other_side_door_name" - attribute, and adds that edge if found. + Adds a new vertex to an existing graph, updating its attributes and edges + based on node-specific data. Args: - node (igraphNode): Passed in to add the vertex to the graph. + node (igraph.Node): Used to insert a new vertex into the graph. """ - self.graph.add_vertex(name=node.name, id=node.id, type=node.type) + 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) @@ -724,9 +756,9 @@ 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 @@ -734,15 +766,15 @@ def insert_igraph_vertex(self, node): def insert_dsr_vertex(self, parent_name, node): # print("Inserting vertex", node["name"], node["type"]) """ - Takes in a parent node name and a node object, creates a new node with the - appropriate attributes, and inserts it into the graph using the `GenericWorker` - class's `insert_node` method. + Inserts a new node into a graph, updating the parent node's attribute with + the agent ID and copying over non-required attributes from the input node + to the new node. Args: - parent_name (str): Used to identify the parent node for the new vertex - being inserted. - node (Node): Represented as a Python dictionary containing the node's - attributes and values, such as agent ID, type, and name. + parent_name (str): Used to specify the name of the parent node to which + the new node will be added. + node (Node | dict): Passed the attributes and data of a vertex to be + inserted into a graph. """ new_node = Node(agent_id=self.agent_id, type=node["type"], name=node["name"]) @@ -759,22 +791,25 @@ def insert_dsr_vertex(self, parent_name, node): id_result = self.g.insert_node(new_node) def insert_igraph_edge(self, edge): """ - Modifies an existing graph by adding a new edge between two nodes based - on edge attributes. + Adds an edge to a long-term graph based on a given edge attribute, updating + the node positions and rotation accordingly. Args: - edge (igraphEdge): Passed as an instance of the Edge class, representing - an edge in the graph with specified attributes. + edge (rt.Edge): Passed by reference to the method, representing an + edge with attributes such as translation and rotation. """ - origin_node = self.g.get_node(edge.origin) - destination_node = self.g.get_node(edge.destination) + 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(name=origin_node.name) - destination_node = self.graph.vs.find(name=destination_node.name) + 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: + self.long_term_graph.g.add_edge(origin_node, destination_node, rt=edge.attrs["rt_translation"].value, 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) @@ -784,14 +819,15 @@ def insert_dsr_edge(self, org, dest): # print("ORG::", org) # self.insert_dsr_vertex(dest) """ - Modifies an edge in a graph based on input arguments 'org' and 'dest'. It - creates a new edge with the appropriate RT value and rotation, and inserts - it into the graph. + Modifies an existing graph by adding an edge with specific RT attributes, + based on input org and dest nodes. Args: - org (Agent): Used to represent the starting node of the edge. - dest (Agent): Used to represent the destination node of the edge being - inserted. + org (GraphNode | NoneType): Used to represent the origin node of the + edge to be inserted. If it is None, the function will use the root + node as the origin. + dest (Node | str): Used to specify the destination node or name in the + graph for insertion of an edge. """ if org is None: @@ -801,8 +837,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"] @@ -821,45 +857,13 @@ def insert_dsr_edge(self, org, dest): # print("Rotation", orientation) self.g.insert_or_assign_edge(new_edge) - def draw_graph(self): - """ - Draws the graph based on the layout "kamada_kawai", plots edges, and - displays node names using 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): """ - Within SpecificWorker, a subclass of GenericWorker, retrieves the room ID - associated with a given node ID by querying the attribute "room_id" and - returning its value upon success or -1 on error. + Determines the room number of an element by retrieving the attribute + 'room_id' from the element's node ID. Args: - node_id (str): Passed as an argument to the function, representing the - unique identifier of a Node in the graph. + node_id (str): Used to reference a node in the graph. Returns: int: The room ID associated with a given node ID. @@ -875,15 +879,15 @@ def check_element_room_number(self, node_id): def check_element_level(self, node_id): """ - Within the `SpecificWorker` class, checks the "level" attribute of a - specified node in the graph and returns its value. If the attribute is not - found, it prints an error message and returns -1. + Determines the element level of a node based on its attribute "level" and + returns it if found, or -1 otherwise. Args: - node_id (str): Used to identify a node in the graph. + node_id (int): Represented by the identifier `node_id`. It serves as + an index into the graph's node list to access the specified node. Returns: - int: Element level of the node with the given ID + int: Element level of the node with given id """ node = self.g.get_node(node_id) @@ -931,14 +935,13 @@ def check_element_level(self, node_id): def generate_room_picture(self, room_node_id): """ - Within the SpecificWorker class retrieves the room ID of a given node, - then checks if there are any RT edges connecting to that node. If such an - edge is found, it extracts the translation attribute and prints it. Finally, - it draws the room polygon and doors using OpenCV. + 1) retrieves room information from the graph, 2) identifies RT edges + connecting to the current room, and 3) extracts translation attribute for + the current room. Args: - room_node_id (str): Used to identify the node representing the room - for which the picture is being generated. + room_node_id (str | int): Required, indicating that it is mandatory + to pass this value for the function to work correctly. """ room_node = self.g.get_node(room_node_id) @@ -977,13 +980,13 @@ def generate_room_picture(self, room_node_id): def insert_current_edge(self, room_id): # Insert current edge to the room """ - Inserts or assigns an edge with specified attributes to the graph represented - by the `self.g` attribute, using the `insert_or_assign_edge` method provided - by the Graph class. + Inserts or assigns an edge in the graph representing the room hierarchy, + with the current worker's ID as the source and the current room ID as the + target. Args: - room_id (str): Used to identify the current room of the agent that is - performing the action. + room_id (str): Used as an identifier for the current room that the + agent is located in. """ current_edge = Edge(room_id, room_id, "current", self.agent_id) @@ -1000,14 +1003,16 @@ def update_node_att(self, id: int, attribute_names: [str]): def update_node(self, id: int, type: str): """ - Updates a node's properties based on its type and other considerations, - such as checking if a door node has a pre-transition edge, inserting a new - vertex or edge in the graph if necessary, and drawing the graph. + Updates a node in the graph based on its ID and type. If the type is "door", + it inserts a vertex in the long-term graph, checks if a room node exists, + and inserts an edge with a specific translation and rotation. If the ID + is the affordance node active ID, it checks the state and activeness of + the node and transitions to the crossed state. Args: - id (int): Used to identify the node to be updated. - type (str): Used to determine the action taken on the node based on - its name. + id (int): Used to identify the node being updated. + type (str): Used to identify the node being updated, with possible + values of "door" or "room". """ if type == "door": @@ -1016,7 +1021,7 @@ def update_node(self, id: int, type: str): 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) + door_igraph = self.long_term_graph.g.vs.find(name=door_node.name) # print("Door node found in igraph. Returning.") return except: @@ -1024,7 +1029,7 @@ def update_node(self, id: int, type: str): # Get room node room_id = door_node.attrs["room_id"].value try: - room_node = self.graph.vs.find(name="room_" + str(room_id)) + 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) @@ -1042,7 +1047,7 @@ def update_node(self, id: int, type: str): 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) + pickle.dump(self.long_term_graph.g, f) print("Door inserted in igraph") self.long_term_graph.draw_graph(False) except Exception as e: @@ -1069,15 +1074,16 @@ def update_edge(self, fr: int, to: int, type: str): # TODO: Posible problema si tocas el nodo room en la interfaz gráfica """ - Updates the current edge of a room node based on certain conditions. If - the target node is the robot id, and there are no edges of type "current" - in the graph, the function inserts the current edge and sets it as current. + Updates the current edge based on the specified frame, to node, and type. + If no current edge exists, it inserts a new edge and sets it as the current + one. Args: - fr (int): Referred to as "from room" in the code snippet provided. - to (int): A reference to an integer representing a node ID in the graph. - type (str): Used to indicate the type of edge being updated, specifically - "RT". + fr (int): Representative of the 'from' node value, which indicates the + starting point of an edge + to (int): The target node ID of an edge to be updated. + type (str): Used to specify the type of edge being updated, either + "RT" for real-time or "NRT" for non-real-time. """ 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: 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') From 877cf2662c076849d9287f862a4741e504ac2524 Mon Sep 17 00:00:00 2001 From: "komment-ai[bot]" <122626893+komment-ai[bot]@users.noreply.github.com> Date: Wed, 17 Jul 2024 07:58:09 +0000 Subject: [PATCH 08/10] Added comments to 41 functions across 3 files --- .komment/00000.json | 840 ++++++++++-------- .komment/komment.json | 5 +- agents/g2o_agent/src/specificworker.py | 336 ++++--- .../src/long_term_graph.py | 54 +- .../src/specificworker.py | 431 +++++---- 5 files changed, 901 insertions(+), 765 deletions(-) diff --git a/.komment/00000.json b/.komment/00000.json index d99aa9a7..1141c341 100644 --- a/.komment/00000.json +++ b/.komment/00000.json @@ -4,42 +4,42 @@ "path": "agents/long_term_spatial_memory_agent/src/long_term_graph.py", "content": { "structured": { - "description": "a class `Room` that represents a 2D room with walls, doors, and objects. It uses the Graphviz library to visualize the room using the DOT language. The code has several methods for computing various aspects of the room, such as getting the center point of the room, drawing walls and doors, and computing the projective coordinates of objects in the room frame.\n\nThe `get_room_corners` method computes the corners of a room by traversing the room graph and finding all nodes with type \"corner\". It then returns a list of these nodes. The `get_room_objects_coordinates` method computes the projective coordinates of objects in the room frame by recursively traversing the room graph, finding all nodes with a given object type, and computing the rotation matrix and rotation vector for each object using the shortest path algorithm.\n\nOverall, this code provides a flexible framework for representing and manipulating 2D rooms with various features, such as walls, doors, and objects, using Graphviz for visualization.", + "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": "a5b8dc06-fd09-cd98-cb49-455fa8997129", + "id": "e77351db-7a15-56b3-3644-4399c887d62b", "ancestors": [], - "description": "Acts as a plotter for a graph representation of a metric reconstruction. It provides methods to draw rooms and doors, and to customize the plot's appearance.", + "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, defaulting to 'r-' for non-current rooms and 'g-' for current rooms." + "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[[Union[str,Path],int],Graph]", - "description": "Used to read a graph from a file specified by the file path or name." + "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 instance for visualization of the graph." + "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 the axis of the graph. It provides methods for setting various properties of the axes, such as titles, labels, limits, and annotations." + "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 second figure object that is created for visualizing the metric reconstruction." + "description": "Used to store the figure object for the second graph." }, { "name": "ax_2", - "type_name": "matplotlibpyplotAxes", - "description": "Used to access and manipulate the second axes object created by the `subplot()` function in the parent plot." + "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", @@ -51,26 +51,26 @@ "comment": null }, "item_type": "class", - "length": 271, + "length": 269, "docLength": null }, { - "id": "a7b64245-15fe-bab5-fc4f-fd7f097e041e", + "id": "35f455db-0eef-1ba5-f04f-cb6cacf60abb", "ancestors": [ - "a5b8dc06-fd09-cd98-cb49-455fa8997129" + "e77351db-7a15-56b3-3644-4399c887d62b" ], - "description": "Reads a graph from a file and creates an instance of the Graph class, optionally creating a figure and axes to visualize the graph.", + "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 path to a graph file from which the graph will be read." + "description": "Used to specify the name of the graph file that contains the LTSM data to be reconstructed." } ], "returns": null, "usage": { "language": "python", - "code": "# Calling the initialization method of the class LongTermGraph with file_name as a parameter\nltg = LongTermGraph('file.txt')\n\n# Accessing and using the graph instance read from the file\nprint(ltg.g)\nprint(ltg.g.summary())\n", + "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__", @@ -86,28 +86,28 @@ "docLength": null }, { - "id": "99491a89-ca41-e68a-5e46-7cd175e3030f", + "id": "252c67c9-d429-faa7-ef4a-8d465711c8a0", "ancestors": [ - "a5b8dc06-fd09-cd98-cb49-455fa8997129" + "e77351db-7a15-56b3-3644-4399c887d62b" ], - "description": "Creates a graph based on a directed graph represented by a NetworkX graph object, and displays it using Matplotlib's scatter and plot functions.", + "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 the nodes and edges of the graph according to their types, only including rooms and doors." + "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": "import matplotlib.pyplot as plt\nfrom LongTermGraph import LongTermGraph\n\n# Create a graph object from a file\ngt = LongTermGraph(\"path/to/file\")\n\n# Draw the graph with only rooms and doors\ngt.draw_graph(only_rooms=True)\n\n# Update the plot\nplt.show()\n", + "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": 254, - "insert": 255, + "start": 252, + "insert": 253, "offset": " ", "indent": 8, "comment": null @@ -251,187 +251,192 @@ "path": "agents/g2o_agent/src/specificworker.py", "content": { "structured": { - "description": "A class `RosOdometry` that handles robot odometry data from a ROS node. It has various methods for updating and deleting nodes and edges in the graph, as well as handling different types of updates such as current position, RT translation, and room changes. The code uses the `graph` package to handle the graph structure and the `rospy` package for ROS-related functionality.", + "description": "a class called `RtOdometry` that implements an algorithm for estimating the position and orientation of a robot based on its odometry data. The code uses the `gazebo` package to simulate the robot's environment and perform odometry calculations. It also utilizes the `rosbag` package to read and process ROS bag files containing sensor data.\n\nThe algorithm first initializes the robot's position and orientation based on the starting pose of the simulation. Then, it updates the node and edge information in the graph using the odometry data from the ROS bag file. Specifically, it calculates the current side speed and angular speed of the robot, and updates the node's position and orientation based on these values. It also updates the edges in the graph based on the current edge set and the RT transmission information.\n\nThe code then implements a few methods for updating and deleting nodes and edges in the graph, as well as updating the algorithm's state based on new sensor data. Overall, the code provides a basic implementation of an odometry algorithm that can be used to estimate the position and orientation of a robot in a simulated environment.", "items": [ { - "id": "81834b2b-13a8-409b-7041-ea7aad5792ec", + "id": "cdcb03d8-8236-2faa-f349-0e323183c24c", "ancestors": [], - "description": "Manages a specific robot's state and updates its graph, edges, and attributes based on messages from the occupancy grid. It also handles room changes and RT sets for the specified robot.", + "description": "Manages a specific worker's tasks and updates in a graph. It has methods for updating node attributes, deleting nodes, and setting up RT transmission, as well as checking for updates and handling errors.", "attributes": [ { "name": "Period", "type_name": "float|int", - "description": "200 by default, indicating the time interval (in milliseconds) between updates of the graph. It can be modified to adjust the update rate of the worker." + "description": "Used to control the update rate of the graph. It determines how often the graph is updated, with a higher value resulting in more frequent updates." }, { "name": "agent_id", "type_name": "int|str", - "description": "A unique identifier for the agent, which can be used to identify specific agents within the system." + "description": "Used to store the ID of the agent that will be using the specific worker." }, { "name": "g", "type_name": "Graph|QGraphicsScene", - "description": "Used to store and manipulate the graph data structure." + "description": "Used to store the graph data for the worker's algorithm, including the nodes, edges, and their attributes." }, { "name": "startup_check", "type_name": "QTimersingleShot200,QApplicationinstancequit", - "description": "Set to call the quit function of the QApplication instance after a delay of 200 milliseconds." + "description": "Used to check if the application is still running after a certain time period (200 milliseconds) and quit if so." }, { "name": "rt_api", - "type_name": "Union[int,str]", - "description": "Used to store the API for the Robot Translation (RT) functionality in the worker. It can take the value of either an integer representing the RT algorithm or a string representing the RT algorithm name." + "type_name": "Union[str,int]", + "description": "Used to store the RT (Real-time) edge set ID for the worker's specific room." }, { "name": "inner_api", - "type_name": "Dict[str,Any]", - "description": "Used to store additional internal data for the worker, such as the current room ID or the first RT set. It is only accessible within the worker's implementation and is not part of the public API." + "type_name": "Callable[[],None]", + "description": "Used to define a custom API for the worker to interact with its internal data structures and algorithms without exposing them to the outer world." }, { "name": "odometry_node_id", "type_name": "int", - "description": "Used to identify the node in the graph that represents the robot's position and orientation." + "description": "Used to identify the node in the graph that represents the robot's odometry information." }, { "name": "odometry_queue", "type_name": "List[Tuple[float,float,float,int]]", - "description": "Used to store the latest odometry data from the robot's sensors for graph updating." + "description": "Used to store odometry data from the environment, containing the robot's current advance speed, side speed, and angular speed, as well as the timestamp in milliseconds since the epoch." }, { "name": "last_odometry", "type_name": "float|List[float]", - "description": "Used to store the last known odometry information of the robot, including its position, velocity, and angular velocity, which are updated every 200 milliseconds." + "description": "Used to store the last known odometry values (position, orientation, and translation) of the robot, which are updated in real-time during the simulation." }, { "name": "g2o", - "type_name": "Graph|npndarray", - "description": "Used to represent the graph of the environment, allowing the worker to perform operations on it such as adding or deleting nodes and edges." + "type_name": "Graph|Tuple[str,str]", + "description": "Used to represent the graph structure of the robot's environment. It contains a tuple of two elements: the first element is the name of the graph file, and the second element is the name of the node file." + }, + { + "name": "visualizer", + "type_name": "Callable[[QWidget],None]", + "description": "Used to create a QWidget instance for visualizing the robot's movement and orientation in real-time." }, { "name": "odometry_noise_std_dev", "type_name": "float|int", - "description": "Used to represent the standard deviation of noise in the odometry measurements. It determines how much the actual robot position, orientation, and velocity are expected to deviate from their predicted values based on the odometry measurements." + "description": "0.1 by default, representing the standard deviation of noise added to the odometry measurements for the robot's position, orientation, and angular velocity." }, { "name": "odometry_noise_angle_std_dev", - "type_name": "float|int", - "description": "1.0 by default, which represents the standard deviation of the noise angle in the odometry measurement. It helps to quantify the uncertainty in the estimated angle of the robot's movement." + "type_name": "float|double", + "description": "0.2 by default, representing the standard deviation of angle noise in the odometry estimates. It affects how much the worker's estimate of the agent's position and orientation deviates from the true values due to random noise in the sensor readings." }, { "name": "measurement_noise_std_dev", "type_name": "float|int", - "description": "Used to represent the standard deviation of measurement noise in the robot's odometry readings. It is used to estimate the uncertainty of the robot's position and velocity." + "description": "0.1 by default, indicating the standard deviation of the noise present in the robot's measurements." }, { "name": "last_room_id", - "type_name": "str|int", - "description": "Used to store the last room ID seen by the agent before changing rooms, used for updating the RT set." + "type_name": "int|str", + "description": "Used to store the room ID of the previous room that the agent has entered before changing rooms, which is useful for determining when the agent has moved to a new room." }, { "name": "actual_room_id", - "type_name": "str|int", + "type_name": "int|str", "description": "Used to store the current room ID of the agent, which is updated when the agent moves to a new room." }, { "name": "elapsed", "type_name": "float|int", - "description": "Used to keep track of the time since the last call to the `update()` method, which is used to control the frequency of updates to the graph." + "description": "Used to store the time since the last call to the `update()` method, which is used to update the node's attributes." }, { "name": "room_initialized", "type_name": "bool", - "description": "Set to False when a room change is detected, indicating that the worker has switched rooms. It is then reset to True once the new room's ID is determined." + "description": "Set to False when a room change is detected, indicating that the worker has not yet initialized its current room's graph. It is reset to True once the worker has successfully initialized the new room's graph." }, { "name": "iterations", - "type_name": "int|List[int]", - "description": "Used to keep track of the number of iterations of the worker's function that have been performed, or the list of iteration numbers if the function is called multiple times with different inputs." + "type_name": "int|float", + "description": "Used to keep track of the number of iterations (i.e., frames) that the worker has processed. It is updated with each frame processed by the worker, and can be used to control the speed of the worker or to implement various algorithms." }, { "name": "hide", "type_name": "bool", - "description": "Used to hide the worker from the graphical user interface (GUI) when set to True." + "description": "Used to indicate whether the worker should be hidden or not, affecting its visibility in the graph." }, { "name": "init_graph", "type_name": "bool", - "description": "Used to track whether the graph has been initialized by the worker during its lifetime. It is used to prevent unnecessary work from being performed when the graph has already been initialized." + "description": "Used to indicate whether the graph has been initialized or not. It is set to True when the worker is initialized and False otherwise, allowing for proper handling of the graph and its nodes and edges in the update methods." }, { "name": "current_edge_set", "type_name": "bool", - "description": "Used to indicate whether the current edge being processed is part of a RT set or not. It is set to True when a new RT edge is encountered, and False otherwise." + "description": "Used to track whether an edge set has been computed for the current room id. It is set to True when the first RT edge set is computed and False otherwise, allowing the worker to avoid recomputing the edge set if it has already been computed for the same room id." }, { "name": "first_rt_set", "type_name": "bool", - "description": "Set to True when the robot performs its first RT (Real-time) movement, indicating that the worker has started tracking RT movements." + "description": "Set to `True` when the RT translation and rotation values are being set for the first time, and `False` otherwise." }, { "name": "translation_to_set", "type_name": "float|List[float]", - "description": "Used to store the translation value to set for the shadow robot. The list stores the values of the translation in the x, y, and z dimensions respectively." + "description": "Used to store the translation values set by the RT algorithm." }, { "name": "rotation_to_set", "type_name": "float|int", - "description": "Used to store the rotation of the robot to set its position relative to its starting point." + "description": "A value representing the rotation of the robot to set its end effector at a specific location." }, { "name": "room_polygon", - "type_name": "Tuple[float,float,float,]", - "description": "Used to store the coordinates of a room's polygon in a graph." + "type_name": "List[float]", + "description": "Used to store the vertices of a polygon that represents the room boundary in the environment." }, { "name": "security_polygon", - "type_name": "Polygon|List[Point]", - "description": "Used to store the security polygon of a room, which is used for collision detection and avoidance during robot navigation." + "type_name": "Union[int,List[float]]", + "description": "Used to store the security polygon of the worker's workspace in 3D space. It is used to detect collisions between the worker's manipulator and objects in the environment." }, { "name": "initialize_g2o_graph", - "type_name": "void|QTimersingleShot200,QApplicationinstancequit", - "description": "Used to initialize a Graph2O graph for representing the environment." + "type_name": "void", + "description": "Used to create a new graph for the given worker instance, which will be used to store the robot's position and orientation in the environment." }, { "name": "rt_set_last_time", "type_name": "int|float", - "description": "Used to track the time since the last RT (Room To) set by the agent. It is used to determine when to reset the translation and rotation to set values." + "description": "Used to store the last time a RT (Real-Time) edge was set for a specific agent ID. It is used in conjunction with the `rt_time_min` attribute to determine when to reset the RT translation and rotation values." }, { "name": "rt_time_min", - "type_name": "float|int", - "description": "Defined as a minimum time gap between two successive RT edge sets, used to determine when to update the translation and rotation values for the agent." + "type_name": "float", + "description": "Defined as the minimum time duration between two consecutive RT sets in milliseconds. It is used to determine when to reset the translation and rotation values for the shadow agent during RT tracking." }, { "name": "last_update_with_corners", "type_name": "int|bool", - "description": "Used to keep track of when the worker has last updated its state with corners data. It is initially set to True, indicating that the worker has not yet received any corners data, and then is updated to False whenever the worker receives new corners data." + "description": "Used to keep track of the last time a node or edge update was performed with corner information. It is initially set to False, and its value changes when updates are performed with corners. Its purpose is to check if the worker has updated with corners before, so as to avoid unnecessary updates in the future." }, { "name": "timer", - "type_name": "int|float", - "description": "Used to track the time elapsed since the last update of the graph, with the purpose of checking if it's been a certain amount of time (200ms) since the last update, and if so, quit the application." + "type_name": "float|int", + "description": "Used to schedule a call to the `QApplication.instance().quit()` function after 2 seconds." }, { "name": "compute", - "type_name": "Callable[[float,float],float]", - "description": "Used to compute the next node ID for the worker's node. It takes two arguments: the current time and a seed value, and returns the next node ID as a float value." + "type_name": "Callable[[],Any]", + "description": "Used to define a function that computes the worker's task based on the graph and other attributes." }, { "name": "update_node_att", - "type_name": "Tuple[int,str,int,int]", - "description": "Used to update the attributes of a node in the graph when the node's ID matches the given ID. The attribute takes four arguments: the node ID, the attribute names, the old value, and the new value." + "type_name": "Callable[int,[str]]", + "description": "Used to update the attributes of a specific node in the graph based on its ID and the type of update. It takes two arguments: id, which is the ID of the node to be updated, and attribute_names, which is a list of attribute names to be updated." }, { "name": "update_edge", - "type_name": "Callable[float,float,str]", - "description": "Used to update the edge attributes of a graph based on specific conditions. It takes three arguments: the first is the source node ID, the second is the target node ID, and the third is the edge type. The attribute is called whenever a new edge is added or an existing edge is modified in the graph, and it can be used to set or update edge attributes based on specific conditions." + "type_name": "Callable[int,str,str]", + "description": "Used to update the attributes of an edge in the graph based on its type (current or RT translation) and the node it connects." }, { "name": "update_edge_att", - "type_name": "List[str]", - "description": "Used to update the attributes of an edge based on a specific type and name. It takes three parameters: the first is the id of the edge, the second is the type of edge, and the third is a list of attribute names to be updated." + "type_name": "Tuple[str,]", + "description": "Used to update the attributes of an edge in a graph based on its type and the name of the attribute." } ], "name": "SpecificWorker", @@ -443,31 +448,31 @@ "comment": null }, "item_type": "class", - "length": 421, + "length": 425, "docLength": null }, { - "id": "dff97ec1-552a-9187-a548-c33a4b688e14", + "id": "d8d35337-69aa-7fa4-5141-35f2de87d6c9", "ancestors": [ - "81834b2b-13a8-409b-7041-ea7aad5792ec" + "cdcb03d8-8236-2faa-f349-0e323183c24c" ], - "description": "Initializes the worker's internal state, including its graph, odometry queue, and other components essential for its operation.", + "description": "Initializes instance variables and sets up event handling for signals related to node and edge attributes, as well as a timer to compute the worker's output every `Period` seconds.", "params": [ { "name": "proxy_map", "type_name": "Dict[str, Any]", - "description": "Used to pass additional data to the SpecificWorker object." + "description": "Used to store the mapping between the original node attributes and their proxies." }, { "name": "startup_check", "type_name": "bool", - "description": "Used to check if the graph has been properly initialized before starting the worker's computation. If set to `False`, it will skip this check and proceed with the computation." + "description": "Used to check if the graph has already been initialized before starting the worker's execution." } ], "returns": null, "usage": { "language": "python", - "code": "from SpecificWorker import SpecificWorker\n\n# Create an instance of the SpecificWorker class, passing in necessary arguments and initializing variables.\nproxy_map = {}\nworker = SpecificWorker(proxy_map)\n\n# Use the init method to initialize any necessary variables or perform any initialization actions that need to be done once when the object is created.\nworker.init()\n\n# Use the compute method to perform any computations required by the program and update any necessary variables.\nworker.compute()\n", + "code": "# Create an instance of the SpecificWorker class\nmy_specific_worker = SpecificWorker(proxy_map)\n\n# Set up the startup check\nmy_specific_worker.startup_check()\n\n# Connect signals to update node attributes and edges\nsignals.connect(my_specific_worker.g, signals.UPDATE_NODE_ATTR, my_specific_worker.update_node_att)\nsignals.connect(my_specific_worker.g, signals.UPDATE_EDGE, my_specific_worker.update_edge)\n", "description": "" }, "name": "__init__", @@ -479,20 +484,20 @@ "comment": null }, "item_type": "constructor", - "length": 54, + "length": 56, "docLength": null }, { - "id": "494ffcaf-17f0-9292-5440-6391c5fd6330", + "id": "8a46ef7f-7071-d3b0-3741-830cddb99e31", "ancestors": [ - "81834b2b-13a8-409b-7041-ea7aad5792ec" + "cdcb03d8-8236-2faa-f349-0e323183c24c" ], - "description": "Processes robot odometry data and updates the graph using G2O optimization to estimate the robot's position and orientation.", + "description": "Computes and updates the robot's position, orientation, and covariance matrix based on RT messages received from the environment.", "params": [], "returns": null, "usage": { "language": "python", - "code": "# Example usage of the Python function compute\ndef main():\n # Create a new instance of the SpecificWorker class\n specific_worker = SpecificWorker(proxy_map)\n\n # Set some initial values for the worker's attributes\n specific_worker.agent_id = 20\n specific_worker.g = DSRGraph(0, \"G2O_agent\", specific_worker.agent_id)\n\n # Call the compute function with a few arguments\n specific_worker.compute(args=[\"--input-file\", \"/path/to/input_file.csv\"])\n\n# Call the main function to run the example code\nif __name__ == \"__main__\":\n main()\n", + "code": "import time\nfrom specific_worker import SpecificWorker\n\n# Create a SpecificWorker object\nspecific_worker = SpecificWorker()\n\n# Call the compute method\nspecific_worker.compute(time.time())\n", "description": "" }, "name": "compute", @@ -504,29 +509,29 @@ "comment": null }, "item_type": "method", - "length": 101, + "length": 103, "docLength": null }, { - "id": "2140c48f-ed84-5b99-2545-c6c54962a658", + "id": "be89d23a-daac-3db1-8843-67ae2a27d66e", "ancestors": [ - "81834b2b-13a8-409b-7041-ea7aad5792ec" + "cdcb03d8-8236-2faa-f349-0e323183c24c" ], - "description": "1) initializes the g2o graph based on room and robot nodes, 2) adds nominal corners for rooms and doors, and 3) fixes the pose of a robot node using odometry information.", + "description": "Initializes a Graph-Based Odometry (G2O) graph for a specific room by:\n\n1. Retrieving node and edge information from a robot's sensor data.\n2. Filtering out invalid nodes and edges based on their position and orientation.\n3. Adding nominal corners and fixed poses to the G2O graph using the retrieved data.", "params": [], "returns": { "type_name": "bool", - "description": "1 if the g2o graph is successfully initialized with nominal corners and fixed poses, and 0 otherwise." + "description": "True if the g2o graph is successfully initialized and False otherwise." }, "usage": { "language": "python", - "code": "import SpecificWorker\n\n# Create a new instance of the worker class and call its initialize method with some arguments.\nworker = SpecificWorker(proxy_map=True, startup_check=False)\nworker.initialize_g2o_graph()\n\n# Some time later, use the g2o graph object to perform operations such as adding a new node or edge.\nworker.g2o.add_node(name='NewNode')\n", + "code": "from genericworker import SpecificWorker\n\n# Create a graph and assign it to g2o variable\ng = DSRGraph(0, \"G2O_agent\", agent_id)\ng2o = G2OGraph(verbose=False)\n\n# Use initialize_g2o_graph function from genericworker module\nSpecificWorker.initialize_g2o_graph(g2o)\n", "description": "" }, "name": "initialize_g2o_graph", "location": { - "start": 252, - "insert": 255, + "start": 253, + "insert": 256, "offset": " ", "indent": 8, "comment": null @@ -536,31 +541,31 @@ "docLength": null }, { - "id": "196e3e68-4c11-b38e-7b41-719b5edd6101", + "id": "9cb9cdad-12a5-6da4-1440-34306b05a309", "ancestors": [ - "81834b2b-13a8-409b-7041-ea7aad5792ec" + "cdcb03d8-8236-2faa-f349-0e323183c24c" ], - "description": "Calculates the displacement of the robot in three dimensions (lateral, forward, and angular) based on the odometry data stored in a queue.", + "description": "Computes the displacement of a robot based on its odometry data, taking into account the advance, lateral movement, and angular velocity of the robot.", "params": [ { "name": "odometry", "type_name": "Tuple[float, float, float]", - "description": "A sequence of 3-element tuples representing the robot's position, velocity, and timestamp in a time interval." + "description": "An immutable sequence containing the robot's odometry data at each time step, consisting of the linear displacement, lateral displacement, and angular velocity, respectively." } ], "returns": { "type_name": "Tuple[float,float,float]", - "description": "3 floating-point values representing the lateral displacement, forward displacement, and angular displacement of an object over a given time interval." + "description": "The displacement along the x, y and z axes, respectively, calculated using the odometry data from a robot's sensors." }, "usage": { "language": "python", - "code": "specific_worker = SpecificWorker(proxy_map=SomeProxyMap)\ndesplazamiento_lateral, desplazamiento_avance, desplazamiento_angular = specific_worker.get_displacement(odometry=SomeOdometry)\nprint(\"Displacement:\", displacement)\n", - "description": "" + "code": "# Create an instance of SpecificWorker class and set the proxy map\nspecific_worker = SpecificWorker(proxy_map)\n\n# Set the last odometry to a value\nlast_odometry = (1, 2, 3, 4)\n\n# Call the get_displacement function with the last odometry as an argument\ndisplacement = specific_worker.get_displacement(last_odometry)\n", + "description": "\nIn this example, we create an instance of the SpecificWorker class and set its proxy map. We then set the last odometry to a value and call the get_displacement function with that value as an argument. The function returns the displacement calculated from the odometry data." }, "name": "get_displacement", "location": { - "start": 457, - "insert": 458, + "start": 458, + "insert": 459, "offset": " ", "indent": 8, "comment": null @@ -570,31 +575,31 @@ "docLength": null }, { - "id": "6c21ce38-1c27-049b-984f-6be77c359b75", + "id": "e5fedf8a-150c-fd98-d148-a6df1ea4a074", "ancestors": [ - "81834b2b-13a8-409b-7041-ea7aad5792ec" + "cdcb03d8-8236-2faa-f349-0e323183c24c" ], - "description": "Computes the covariance matrix between a given vertex and all other vertices in the graph, using the G2O optimizer to compute marginals and upper triangle matrix representation.", + "description": "Computes the covariance matrix of a set of vertices in a graph, using an optimization algorithm to compute the marginals of the vertices and then constructing the covariance matrix from the resulting upper triangle.", "params": [ { "name": "vertex", "type_name": "G2O.Vertex | G2O.HessianIndex", - "description": "Used to compute the covariance matrix for a specific vertex in the graph." + "description": "Used to compute the covariance matrix for a specific vertex in a graph." } ], "returns": { - "type_name": "numpyndarray", - "description": "2-dimensional and upper-triangular matrix representing the covariance matrix between a given vertex and all other vertices in the graph." + "type_name": "Tuple[bool,npndarray]", + "description": "A result of computing covariance matrix and the actual computed matrix." }, "usage": { "language": "python", - "code": "# Create an instance of SpecificWorker\nworker = SpecificWorker()\n\n# Call the get_covariance_matrix method with a vertex as argument\nvertex = g2o.VertexSE3(0) # Create a VertexSE3 object with index 0\nresult, cov_mat = worker.get_covariance_matrix(vertex)\n\n# Print the result and covariance matrix\nprint(f\"Result: {result}\")\nprint(f\"Covariance Matrix:\\n{cov_mat}\")\n", + "code": "import sys\nfrom specific_worker import SpecificWorker\n\n# Create a new instance of SpecificWorker\nworker = SpecificWorker()\n\n# Set the proxy map and startup check to True\nproxy_map = \"proxy_map\"\nstartup_check = True\n\n# Initialize the worker with the given parameters\nworker.init(proxy_map, startup_check)\n\n# Get the covariance matrix of a vertex in the graph\nvertex = worker.g2o.graph.vertices[0]\ncovariance_matrix = worker.get_covariance_matrix(vertex)\nprint(\"Covariance matrix:\", covariance_matrix)\n", "description": "" }, "name": "get_covariance_matrix", "location": { - "start": 476, - "insert": 477, + "start": 477, + "insert": 478, "offset": " ", "indent": 8, "comment": null @@ -604,28 +609,28 @@ "docLength": null }, { - "id": "32bdf14a-da4a-c7ae-2443-b9488fb241d4", + "id": "3fa09193-ee9b-8ebd-e44e-8d5f121f7d5a", "ancestors": [ - "81834b2b-13a8-409b-7041-ea7aad5792ec" + "cdcb03d8-8236-2faa-f349-0e323183c24c" ], - "description": "Visualizes the real-time optimization process of a G2O algorithm by plotting the positions and edges of the vertices in 3D space.", + "description": "Visualizes a real-time 3D graphical representation of a G2O optimization problem using Python's Matplotlib library.", "params": [ { "name": "optimizer", - "type_name": "object | Optimizer", - "description": "Used to load G2O files, estimate vertex positions, and visualize them in real-time." + "type_name": "object", + "description": "Used to load G2O files for visualization." } ], "returns": null, "usage": { "language": "python", - "code": "from SpecificWorker import SpecificWorker\nimport matplotlib.pyplot as plt\n\n# Create a new instance of the SpecificWorker class\nworker = SpecificWorker()\n\n# Load a graph file and set up the G2OGraph object\nworker.g2o.load(\"archivo.g2o\")\n\n# Visualize the real-time optimization process using matplotlib's 3D plotting library\nplt.ion()\nfig = plt.figure()\nax = fig.add_subplot(111, projection='3d')\nworker.visualize_g2o_realtime(optimizer)\n", + "code": "import numpy as np\nfrom g2o import *\nfrom g2o_visualizer import G2OVisualizer\n\n# Create a new instance of the SpecificWorker class.\nworker = SpecificWorker(proxy_map=None, startup_check=False)\n\n# Set up the visualizer and G2OGraph for the worker.\nworker.visualizer = G2OVisualizer(\"G2O Graph\")\nworker.g2o = G2OGraph(verbose=False)\n\n# Load a graph file into the worker's G2OGraph instance.\nworker.g2o.load(\"archivo.g2o\")\n\n# Set up the visualization loop for the worker.\nwhile True:\n # Load the graph file and update the visualizer.\n optimizer = worker.g2o\n positions = []\n for vertex_id in range(optimizer.vertices().size()):\n vertex = optimizer.vertex(vertex_id)\n position = vertex.estimate()\n positions.append(position)\n positions = np.array(positions)\n\n worker.visualizer.ax.clear()\n worker.visualizer.ax.scatter(positions[:, 0], positions[:, 1], positions[:, 2], c='b', marker='o', label='Vertices')\n\n edges = optimizer.edges()\n for edge_id in range(edges.size()):\n edge = edges[edge_id]\n measurement = edge.measurement()\n worker.visualizer.ax.plot(measurement[:, 0], measurement[:, 1], measurement[:, 2], c='r')\n\n worker.visualizer.ax.set_xlabel('X')\n worker.visualizer.ax.set_ylabel('Y')\n worker.visualizer.ax.set_zlabel('Z')\n worker.visualizer.ax.legend()\n\n worker.visualizer.draw()\n worker.visualizer.pause(0.1)\n", "description": "" }, "name": "visualize_g2o_realtime", "location": { - "start": 489, - "insert": 490, + "start": 490, + "insert": 491, "offset": " ", "indent": 8, "comment": null @@ -635,33 +640,33 @@ "docLength": null }, { - "id": "db934395-dc97-4b9f-7f41-4d42e8474534", + "id": "595b6b91-9394-1b80-7345-230aff72914c", "ancestors": [ - "81834b2b-13a8-409b-7041-ea7aad5792ec" + "cdcb03d8-8236-2faa-f349-0e323183c24c" ], - "description": "Updates the attributes of a node in a graph, specifically the odometry queue.", + "description": "Updates attributes of a node in a graph based on the current time and robot state, and appends the updated values to an odometry queue for later use.", "params": [ { "name": "id", "type_name": "int", - "description": "Represented as an integer value that identifies the node to be updated." + "description": "Used to identify the node for which the attributes are being updated." }, { "name": "attribute_names", "type_name": "[str]", - "description": "An array containing the names of attributes to be updated for the given node ID." + "description": "An array of strings representing the names of attributes to be updated on the given node ID." } ], "returns": null, "usage": { "language": "python", - "code": "# Import SpecificWorker class from the module specific_worker.py\nfrom specific_worker import SpecificWorker\n\n# Initialize a SpecificWorker object named \"specific_worker\" with proxy map and startup check as True\nspecific_worker = SpecificWorker(proxy_map=True, startup_check=True)\n\n# Update node attribute using the update_node_att method\nspecific_worker.update_node_att(id=20, attribute_names=[\"robot_current_advance_speed\", \"robot_current_side_speed\", \"robot_current_angular_speed\"])\n", - "description": "\nIn this example, we initialize a SpecificWorker object with proxy map and startup check as True. Then, we use the update_node_att method to update the node attribute of the robot with ID 20 by passing in the attribute names that we want to update. The update_node_att method is then executed using the specific_worker object." + "code": "specificWorker = SpecificWorker(proxy_map)\nspecificWorker.update_node_att(id=20, attribute_names=[\"robot_current_advance_speed\", \"robot_current_side_speed\", \"-robot_current_angular_speed\"])\n", + "description": "\nIn this example, the end-user is creating a SpecificWorker instance and calling update_node_att with an ID of 20 and a list of attribute names. The function will retrieve the node from the graph based on the ID provided and update the specified attributes in the node's attribute dictionary." }, "name": "update_node_att", "location": { - "start": 526, - "insert": 535, + "start": 527, + "insert": 536, "offset": " ", "indent": 8, "comment": null @@ -671,33 +676,33 @@ "docLength": null }, { - "id": "adfc3c84-948f-4690-934e-0275aa1375a7", + "id": "e4669eb8-bce7-27aa-de44-d04a35cedb5c", "ancestors": [ - "81834b2b-13a8-409b-7041-ea7aad5792ec" + "cdcb03d8-8236-2faa-f349-0e323183c24c" ], - "description": "Updates a node with ID `id`. The method checks if the node type is \"corner\", and if so, it retrieves the room node from the graph and sets a flag indicating that the room has been initialized.", + "description": "Updates the node with the given ID based on the type parameter. If the type is \"corner\", it initializes a room graph and sets the `init_graph` attribute to `True`.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to identify the node to be updated." + "description": "An identifier for the node to be updated." }, { "name": "type", "type_name": "str", - "description": "Used to specify the node's type." + "description": "Used to specify the node's type, which can be either \"corner\" or any other valid value." } ], "returns": null, "usage": { "language": "python", - "code": "# Assuming you have created an instance of the SpecificWorker class, worker, as shown in the previous code snippet\nworker.update_node(id=100, type='corner') # Updates the node with ID 100 to be a corner\n", - "description": "\nThis example shows how the update_node function can be used to update the specific node with ID 100 to be of type 'corner'." + "code": "worker = SpecificWorker(proxy_map) # Initializes a worker with the proxy map\nid = 5 # Assigns node ID to the variable id\ntype = \"corner\" # Assigns node type to the variable type\nworker.update_node(id, type) # Updates the node in the graph with the given ID and type\n", + "description": "\nIn this example, the worker is initialized by passing the proxy map as a parameter and then the update_node function is called with the id and type arguments assigned to variables outside the function." }, "name": "update_node", "location": { - "start": 542, - "insert": 552, + "start": 543, + "insert": 553, "offset": " ", "indent": 8, "comment": null @@ -707,28 +712,28 @@ "docLength": null }, { - "id": "3e556671-8722-7790-9c4b-f6eb29ed8f0f", + "id": "6e50ec45-7089-e09c-a842-9c0f0714d216", "ancestors": [ - "81834b2b-13a8-409b-7041-ea7aad5792ec" + "cdcb03d8-8236-2faa-f349-0e323183c24c" ], - "description": "Deletes a node from a list or dict based on its ID, setting `self.room_initialized` to `False`.", + "description": "Deletes a node from a list of nodes maintained by the worker.", "params": [ { "name": "id", "type_name": "int", - "description": "Intended to represent the unique identifier for the node being deleted." + "description": "Used to represent the unique identifier of the node to be deleted." } ], "returns": null, "usage": { "language": "python", - "code": "# Create an instance of SpecificWorker class\nworker = SpecificWorker(proxy_map)\n\n# Call delete_node method with node id as parameter\nworker.delete_node(1234)\n", - "description": "" + "code": "# Create an instance of the SpecificWorker class\nworker = SpecificWorker(proxy_map)\n\n# Call the delete_node method with the ID of the node to delete\nworker.delete_node(1234567890)\n", + "description": "\nIn this example, we create an instance of the SpecificWorker class and then call the delete_node method with the ID of the node we want to delete (1234567890). The function will then execute the code to delete the specified node from the graph." }, "name": "delete_node", "location": { - "start": 554, - "insert": 555, + "start": 555, + "insert": 556, "offset": " ", "indent": 8, "comment": null @@ -738,38 +743,38 @@ "docLength": null }, { - "id": "ad7c5a47-c41a-8982-0d4b-b1573ab838c8", + "id": "67728d31-113e-e0b4-c34d-aaa4fd6ab27c", "ancestors": [ - "81834b2b-13a8-409b-7041-ea7aad5792ec" + "cdcb03d8-8236-2faa-f349-0e323183c24c" ], - "description": "Updates the room ID and sets the current edge set based on the type of edge received from the graph.", + "description": "Updates the room ID and RT translation and rotation values based on the incoming edge type and node attributes.", "params": [ { "name": "fr", "type_name": "int", - "description": "Representing the starting node index of an edge in a graph." + "description": "The index of the current edge being processed." }, { "name": "to", "type_name": "int", - "description": "The target node index of the edge to be updated, which can be either a room or the Shadow node." + "description": "Representing the index of the next node in the graph that is being processed." }, { "name": "type", "type_name": "str", - "description": "Used to specify the edge type, which can be either \"current\" or \"RT\"." + "description": "Used to specify the edge's type, which can be either \"current\" or \"RT\"." } ], "returns": null, "usage": { "language": "python", - "code": "# Initialize the SpecificWorker class and its graph g\nspecific_worker = SpecificWorker(proxy_map=ProxyMap())\nspecific_worker.g = DSRGraph()\n\n# Add nodes to the graph\nnode1 = specific_worker.g.add_node(\"Node 1\")\nnode2 = specific_worker.g.add_node(\"Node 2\")\n\n# Add edges between the nodes\nedge = specific_worker.g.add_edge(node1, node2)\n\n# Call the update_edge function to set a new edge type\nspecific_worker.update_edge(fr=node1, to=node2, type=\"current\")\n\n# Update the graph with the new edge type\nspecific_worker.g.update()\n", - "description": "" + "code": "# In the context of the SpecificWorker class, where \"worker\" is an instance of the SpecificWorker class\nworker.update_edge(fr=0, to=1, type=\"current\") # Update the current edge from node 0 to node 1\nworker.update_edge(fr=0, to=1, type=\"RT\") # Update the RT edge from node 0 to node 1\n", + "description": "\nThe update_edge function updates the edge between two nodes in the DSRGraph with a specific type (current or RT). The fr parameter specifies the start node of the edge, while the to parameter specifies the end node." }, "name": "update_edge", "location": { - "start": 563, - "insert": 564, + "start": 564, + "insert": 565, "offset": " ", "indent": 8, "comment": null @@ -779,38 +784,38 @@ "docLength": null }, { - "id": "e0e06e79-6b30-35bd-c340-bdc7b00ca599", + "id": "4825b582-5f59-328e-c944-7eddbc75d3f0", "ancestors": [ - "81834b2b-13a8-409b-7041-ea7aad5792ec" + "cdcb03d8-8236-2faa-f349-0e323183c24c" ], - "description": "Deletes an edge from a graph, specified by its index (fr) and the index of its adjacent vertex (to). The edge's type can be specified for further filtering.", + "description": "Deletes an edge from a graph, identified by its index (fr) and type (to).", "params": [ { "name": "fr", "type_name": "int", - "description": "1st argument or input of the function indicating the first vertex or node to delete an edge from." + "description": "0-based index of the edge to be deleted." }, { "name": "to", "type_name": "int", - "description": "Used to specify the destination vertex ID for the edge deletion operation." + "description": "Used to specify the target vertex for edge deletion." }, { "name": "type", "type_name": "str", - "description": "Used to indicate the edge type to be deleted, with possible values 'in' or 'out'." + "description": "Used to specify the type of edge to delete, with possible values 'direct' or 'indirect'." } ], "returns": null, "usage": { "language": "python", - "code": "# Delete an edge from the graph\nworker.delete_edge(fr=1, to=2, type=\"TYPE\")\n", - "description": "\nIn this example, the `delete_edge` method of the SpecificWorker class is called with three arguments: fr, to, and type. The first two arguments, fr and to, represent the source and target nodes of the edge that should be deleted, respectively. The third argument, type, specifies the type of the edge that should be deleted.\n\nFor instance, if we want to delete an edge with the source node being node 1 and the target node being node 2, and the edge has a type of \"TYPE\", we can call the `delete_edge` method as follows:\n" + "code": "my_graph = SpecificWorker(proxy_map)\n# Create a new edge between two nodes in the graph\nmy_graph.add_edge(0, 1, \"type\")\n# Delete the edge from the graph\nmy_graph.delete_edge(0, 1, \"type\")\n", + "description": "" }, "name": "delete_edge", "location": { - "start": 591, - "insert": 592, + "start": 592, + "insert": 593, "offset": " ", "indent": 8, "comment": null @@ -1223,127 +1228,142 @@ "path": "agents/long_term_spatial_memory_agent/src/specificworker.py", "content": { "structured": { - "description": "An igraph module that simulates a robot's navigation through a room using RT graph theory. It provides several functions for inserting, updating, and deleting nodes and edges in the room graph, as well as querying node and edge attributes. The code also includes logic to set the current state of the robot based on the type of edge inserted or removed from the room graph. The module uses the `igraph` package to manipulate graphs and perform graph theory operations.", + "description": "An igraph wrapper around a graph and provides several methods for working with it, including inserting new nodes and edges, updating edge attributes, and deleting nodes and edges. The code uses the `igraph` package to interface with the graph. Specifically, it creates a long-term graph (LTG) and a short-term graph (STG), allowing for efficient insertion, deletion, and manipulation of nodes and edges in the LTG while maintaining the integrity of the STG. The code also provides functions to update edge attributes and delete nodes or edges from the graph.", "items": [ { - "id": "8012c732-b5dd-93a9-2441-d47f31f3d2fa", + "id": "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b", "ancestors": [], - "description": "Manages a graph representing a manipulation task, providing methods for inserting, updating, and deleting nodes and edges. It also keeps track of the robot's current state and performs actions based on that state.", + "description": "Manages a specific node and edge attributes and performs various operations on them, such as updating, inserting, deleting, and attributing. It also keeps track of the current state of the robot and updates the graph accordingly.", "attributes": [ { "name": "Period", - "type_name": "str|int", - "description": "Used to store the period of time during which the worker performs its task, either in seconds or minutes, depending on the value assigned to it." + "type_name": "Union[int,str]", + "description": "Used to store the period of time during which the worker can perform its task, such as a day or a week." }, { "name": "agent_id", "type_name": "int|str", - "description": "Used to store the ID of the specific worker for which the methods of this class are intended to work." + "description": "Used to identify the specific worker instance, allowing the agent to perform tasks and maintain a unique identity during its lifetime." }, { "name": "g", "type_name": "igraphGraph", - "description": "Used for handling long-term dependencies between nodes in the graph." + "description": "Used to represent the long-term graph of the environment, which is updated periodically by the worker." }, { "name": "startup_check", "type_name": "bool|str", - "description": "Used to check if the worker has started successfully or not. It can be set to a specific value like \"success\" or \"failure\" to indicate whether the startup was successful or not, or it can be left blank to default to a boolean value indicating whether the worker started successfully or not." + "description": "Used to check if the worker has been started or not, with possible values \"yes\", \"no\", or False." }, { "name": "rt_api", "type_name": "str|int", - "description": "Used to store the RT (Real-Time) API of the worker, which allows the robot to perform real-time actions." + "description": "Used for storing the RT (Real-Time) API ID which represents the current task being executed by the worker." }, { "name": "inner_api", "type_name": "Dict[str,Any]", - "description": "Used to store internal APIs for the worker's methods that are not exposed to the outside world." + "description": "Used to store the inner API of the worker, which is a Python function that performs actions related to the worker's task." }, { "name": "robot_name", - "type_name": "str|List[str]", - "description": "Used to store the name of the robot that the worker belongs to." + "type_name": "str|int", + "description": "Used to store the name of the robot that will perform a specific task." }, { "name": "robot_id", "type_name": "int", - "description": "Used to represent the unique identifier of the robot node in the graph." + "description": "Used to identify the robot node in the graph, which is the starting point for the worker's movement." }, { "name": "last_robot_pose", - "type_name": "Dict[str,float]", - "description": "Used to store the last known pose of the robot before it entered a room or crossed a boundary." + "type_name": "Tuple[float,float,float]", + "description": "Stored as the last known pose (position, orientation, and scale) of the robot before any actions were taken in the environment." }, { "name": "robot_exit_pose", "type_name": "str|int", - "description": "Used to store the ID of the room node that the robot exits through when transitioning from a door node to a room node." + "description": "A reference to the pose of the robot when it exits a room. It is used to determine when the robot has reached its goal in a task." }, { "name": "state", - "type_name": "str|bool", - "description": "Used to keep track of the worker's current state (either \"idle\", \"crossed\", or \"completed\")." + "type_name": "str|int", + "description": "Used to keep track of the current state of the worker, which can be either \"idle\", \"crossed\", or \"completed\"." }, { "name": "affordance_node_active_id", "type_name": "int|bool", - "description": "Used to store the ID of the affordance node that is currently active or not." + "description": "Used to keep track of the active affordance node ID, which is used to determine when the robot has reached its goal." }, { "name": "exit_door_id", "type_name": "int|str", - "description": "Used to store the ID of the door node that serves as an exit from a room." + "description": "Used to store the ID of the door node that leads from a room to the outside world." }, { "name": "room_exit_door_id", "type_name": "int|str", - "description": "16 by default, representing the id of a specific door node in the long-term graph that marks the exit of a room." + "description": "Used to represent the ID of the door node that leads from a room to the outside world." }, { "name": "enter_room_node_id", - "type_name": "int|str", - "description": "Represented as a room number that, when updated to a value greater than zero, indicates that the robot has entered a new room." + "type_name": "int", + "description": "Used to store the ID of the room node that the worker is currently entering." }, { "name": "vertex_size", "type_name": "int|str", - "description": "Used to store the size or dimension of a vertex in the graph, which can be used to determine the size of a node in the graph." + "description": "Used to keep track of the number of vertices in the graph, which can be useful for various operations such as insertion, deletion, and querying the graph." }, { "name": "not_required_attrs", "type_name": "List[str]", - "description": "Used to store a list of attributes that are not required for the worker's operations. It helps to optimize the worker's performance by ignoring these unused attributes during updates, inserts, or deletes." + "description": "Used to store a list of attributes that are not required for the worker's functionality, but can be useful for debugging or other purposes." + }, + { + "name": "last_save_time", + "type_name": "datetime|str", + "description": "Used to store the time when the worker's long-term graph was last saved. It helps track when the graph was last updated for subsequent actions." }, { "name": "long_term_graph", "type_name": "igraphGraph", - "description": "Used to store the long-term graph representation of the environment, which is updated during the worker's execution." + "description": "Used to store the long-term graph representation of the environment, which is updated over time as new data becomes available." + }, + { + "name": "initialize_room_from_igraph", + "type_name": "Dict[str,Any]", + "description": "Used to initialize the room graph from an igraph graph. It takes the igraph graph as input and creates a new room graph with the same node IDs and edges as the input igraph." + }, + { + "name": "update_robot_pose_in_igraph", + "type_name": "void", + "description": "Used to update the pose of a robot node in the graph by inserting a new edge with the correct \"RT\" translation and rotation." }, { "name": "insert_current_edge", - "type_name": "NoneOrEdge|List[str]", - "description": "Used to insert a new edge into the graph with the specified fr and to nodes, and sets it as the current edge if no current edge exists." + "type_name": "List[Edge]", + "description": "Used to insert a new edge into the graph with the specified source and target nodes and edge type, effectively setting it as the current edge in the graph." }, { "name": "timer", "type_name": "int|float", - "description": "Used to track the time taken by the worker to process a task." + "description": "Used to track the time taken by the worker to perform its tasks." }, { "name": "compute", - "type_name": "Callable[[str,str],Any]", - "description": "Used to specify a function that computes the next action for the robot based on its current state and the environment." + "type_name": "Callable[[int],str|int]", + "description": "Defined as a method that takes an integer argument and returns a string or integer value representing the result \nof some computation. It is used in various methods of the worker to perform computations on the graph." }, { "name": "update_node", "type_name": "Dict[str,Any]", - "description": "Used to update a node in the graph based on its ID, type (door or room), and name. It performs various actions such as checking if a door node exists, inserting a door node, updating the RT translation and rotation, and drawing the graph." + "description": "Responsible for updating a node in the graph when the worker's id matches the node's id. It takes three parameters: `id`, `type`, and `door_node`. The method updates the node's attributes based on its type and inserts an RT edge between the node and its parent if necessary." }, { "name": "update_edge", - "type_name": "Union[str,int]", - "description": "Used to update the edge attributes of a specific robot node in the graph when the node's door is opened." + "type_name": "Union[int,str]", + "description": "Used to update an edge's type or remove it altogether based on certain conditions." } ], "name": "SpecificWorker", @@ -1355,31 +1375,31 @@ "comment": null }, "item_type": "class", - "length": 600, + "length": 628, "docLength": null }, { - "id": "2420b0f0-67bd-b096-4c4c-57bbc351c392", + "id": "ad42c39d-e8e5-3180-aa4c-67d2bbbe477e", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Initializes instance variables and sets up event listeners for graph updates, timeouts, and node measurements.", + "description": "Initializes the worker's internal state, including its graph, node and edge variables, and timers for computing and updating the graph.", "params": [ { "name": "proxy_map", - "type_name": "Dict[str, Any]", - "description": "Used to pass additional data to the SpecificWorker object. It contains key-value pairs that can be used to store any information required by the worker." + "type_name": "dict", + "description": "Used to pass a mapping of proxy nodes to their corresponding real nodes in the graph." }, { "name": "startup_check", "type_name": "bool", - "description": "Used to check if the agent has already started its computation, skipping the computation if it has already been done." + "description": "Set to False by default. It performs a check on the graph upon initialization if it's set to True, otherwise, it does not." } ], "returns": null, "usage": { "language": "python", - "code": "from SpecificWorker import SpecificWorker\n\n# Initialize a new instance of the class\nspecific_worker = SpecificWorker(proxy_map)\n", + "code": "from SpecificWorker import SpecificWorker\nimport time\n\n# Initialize the worker with a proxy map and a startup check\nworker = SpecificWorker(proxy_map, True)\n\n# Set up the timer for the compute method\ntimer = time.time()\nworker.setPeriod(100)\nworker.startTimer()\n\nwhile True:\n # Perform the computation\n worker.compute()\n \n # Check if it's time to save the graph\n if time.time() - last_save_time > 3600:\n worker.saveGraph(\"graph.pkl\")\n", "description": "" }, "name": "__init__", @@ -1391,20 +1411,20 @@ "comment": null }, "item_type": "constructor", - "length": 51, + "length": 54, "docLength": null }, { - "id": "3cc990dd-edc6-eaa3-e84f-526e79602e4f", + "id": "61093faf-d7f7-cebb-4d49-712ed045066d", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Sets the parameters passed as an argument to `True`. Then, it removes a self-edge from the current room, stores the ID of the exit door in a variable, and sets the name attribute of both doors to the other side door's name.", + "description": "Sets parameters and modifies the state of the room by removing a self-edge, storing an exit door ID, and adding attributes to doors.", "params": [ { "name": "params", "type_name": "bool", - "description": "Passed to set parameters for an object of class `Room`." + "description": "Passed to set the parameters of the room." } ], "returns": { @@ -1413,13 +1433,13 @@ }, "usage": { "language": "python", - "code": "# Initialise a SpecificWorker instance and assign its parameters\nworker = SpecificWorker(proxy_map)\nparams = {\"Period\": 100, \"agent_id\": 13}\nworker.setParams(params)\n\n# Use the worker instance to perform some tasks\nworker.startup_check()\nworker.rt_api.update_node_attributes(worker.g, node_id=worker.robot_id, attr_map={\"timestamp_alivetime\": 1})\n", - "description": "\nIn this example, we first create a new SpecificWorker instance and initialise its parameters using the setParams function. We then use the worker instance to perform some tasks such as startup_check() and updating node attributes with the rt_api object." + "code": "worker = SpecificWorker(proxy_map)\n# setParams takes in a dictionary of parameters as input\nparams = {\"Period\": 10, \"agent_id\": 12}\nworker.setParams(params)\n", + "description": "\nIn this example, the user creates an instance of the SpecificWorker class and passes a dictionary of parameters to the setParams method. The function then updates the worker's internal state based on the input parameters." }, "name": "setParams", "location": { - "start": 124, - "insert": 125, + "start": 127, + "insert": 128, "offset": " ", "indent": 8, "comment": null @@ -1429,47 +1449,97 @@ "docLength": null }, { - "id": "d6fad876-0520-74ba-db4f-01fdb0bda8d3", + "id": "cfc1e69d-8797-9087-e844-76a8a7e76242", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Computes and updates the robot's RT (reactive transport) based on the current state of the graph.", + "description": "Updates the robot's pose in the graph and performs various actions based on the current state of the robot.", "params": [], "returns": null, "usage": { "language": "python", - "code": "import SpecificWorker\n\n# Initialize the SpecificWorker object with the proxy map and a flag for startup check\nspecific_worker = SpecificWorker(proxy_map, startup_check=True)\n\n# Call the compute method to perform the computation\nresult = specific_worker.compute()\n\n# Print the result of the computation\nprint(result)\n", + "code": "specific_worker = SpecificWorker()\nspecific_worker.compute()\n", "description": "" }, "name": "compute", "location": { - "start": 138, - "insert": 141, + "start": 141, + "insert": 144, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 61, + "length": 22, "docLength": null }, { - "id": "8506fabb-8dc7-2aab-f04a-6f0ba0f9cc92", + "id": "8b3d3eed-7010-bd8a-8045-f75833aa77e7", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Computes the affordance pose of the robot in a new room based on its current position and orientation, and associates doors in the long-term graph with their corresponding rooms.", + "description": "Initializes a room in a graph by deleting an edge, finding the robot node and its neighbors, creating a new vertex for the root of the igraph, inserting edges to connect the robot to the room and the room to itself, and updating the robot's parent node attribute.", "params": [], "returns": null, "usage": { "language": "python", - "code": "specific_worker = SpecificWorker(proxy_map) # Initiate a specific worker object\nspecific_worker.idle() # Call the idle method from the specific worker object\n", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nworker.initialize_room_from_igraph()\n", + "description": "\nThis code uses the Python function \"initialize_room_from_igraph\" to initialize the room from an igraph graph structure. The function is called on a worker object created from the SpecificWorker class, which subclasses GenericWorker." + }, + "name": "initialize_room_from_igraph", + "location": { + "start": 165, + "insert": 167, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 22, + "docLength": null + }, + { + "id": "aab2b882-aaae-999f-b248-533b7089da3f", + "ancestors": [ + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + ], + "description": "Updates the robot's pose in an igraph graph based on a RT translation and rotation, taking into account the robot's current location and the surrounding rooms.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "# create an instance of SpecificWorker class\nspecific_worker = SpecificWorker(proxy_map)\n\n# call the update_robot_pose_in_igraph method to update robot pose in igraph\nspecific_worker.update_robot_pose_in_igraph()\n", + "description": "" + }, + "name": "update_robot_pose_in_igraph", + "location": { + "start": 198, + "insert": 200, + "offset": " ", + "indent": 8, + "comment": null + }, + "item_type": "method", + "length": 41, + "docLength": null + }, + { + "id": "67757034-6f36-3890-0942-bd47b1455c70", + "ancestors": [ + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + ], + "description": "Detects when the robot enters a new room and associates doors based on their proximity to the robot's current position, updating the graph and door attributes accordingly.", + "params": [], + "returns": null, + "usage": { + "language": "python", + "code": "# Initialize the worker and specify the startup check parameter as True\nworker = SpecificWorker(proxy_map, startup_check=True)\n\n# Perform an initialization check for the worker\nworker.startup_check()\n\n# Set the agent ID to 13\nworker.agent_id = 13\n\n# Update the robot name to \"Shadow\"\nworker.robot_name = \"Shadow\"\n\n# Update the vertex size to 0\nworker.vertex_size = 0\n\n# Set the state of the worker to idle\nworker.state = \"idle\"\n", "description": "" }, "name": "idle", "location": { - "start": 211, - "insert": 213, + "start": 251, + "insert": 253, "offset": " ", "indent": 8, "comment": null @@ -1479,22 +1549,22 @@ "docLength": null }, { - "id": "04e2af7d-eddb-8f8c-6743-11e3dda21651", + "id": "51b1dbb4-c5a5-63b2-4b43-06106aa87d12", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Updates the state of the worker based on the active affordance node and exit door ID.", + "description": "1) retrieves the active affordance node and its parent, 2) sets the exit door ID to the value of the parent attribute, and 3) deletes the edge connecting the current room to itself.", "params": [], "returns": null, "usage": { "language": "python", - "code": "# create an instance of the SpecificWorker class\nworker = SpecificWorker(proxy_map)\n\n# set the agent's ID and robot name\nagent_id = 13\nrobot_name = \"Shadow\"\n\n# call the crossed method\nworker.crossed()\n", - "description": "\nThe crossed function is a method of the SpecificWorker class that subclasses GenericWorker. It takes no arguments and does not return any value. The function first gets the affordance node with ID self.affordance_node_active_id from the graph g using get_node(). If this node has a parent attribute, it sets the exit door ID to the parent attribute value. If the parent attribute is empty, it returns.\nIf the exit door ID is set, the function deletes an edge between room_exit_door_id and self.room_exit_door_id using delete_edge().\nThe function then gets the exit door node with ID self.exit_door_id from the graph g using get_node(). If this node has a connected_room_name attribute, it sets the state to known_room and prints an INSERTING KNOWN ROOM message. If the exit door ID is empty, it sets the state to initializing_room and prints an INITIALIZING ROOM message.\nNote that the SpecificWorker class also has other attributes such as agent_id, g, rt_api, inner_api, robot_name, last_robot_pose, robot_exit_pose, state, affordance_node_active_id, exit_door_id, room_exit_door_id, enter_room_node_id, and vertex_size." + "code": "import SpecificWorker\n\n# Create an instance of the SpecificWorker class\nworker = SpecificWorker(proxy_map=your_proxy_map)\n\n# Call the crossed method with no arguments and store the return value in a variable\nroom_state = worker.crossed()\n\n# Print the returned value to see if the function executed successfully\nprint(\"Room state:\", room_state)\n", + "description": "" }, "name": "crossed", "location": { - "start": 310, - "insert": 312, + "start": 350, + "insert": 352, "offset": " ", "indent": 8, "comment": null @@ -1504,22 +1574,22 @@ "docLength": null }, { - "id": "789455c6-28e2-7985-854d-e4841c68865e", + "id": "d55c53ae-f2c0-628c-ca4e-0ad084fa17b1", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Initializes the room nodes and sets the enter room node ID, then transitions to the \"initializing doors\" state.", + "description": "Determines and sets the ID of the room node to enter upon construction, based on conditions involving the existence of nodes labeled \"room\" and the absence of certain attributes.", "params": [], "returns": null, "usage": { "language": "python", - "code": "# Initialize the specific worker class with the proxy map and the startup check flag set to False\nspecific_worker = SpecificWorker(proxy_map, startup_check=False)\n\n# Call the initializing_room method on the specific worker instance\nspecific_worker.initializing_room()\n", + "code": "from mymodule import SpecificWorker\n\n# Creating an instance of the class and using its method initializing_room\nmyworker = SpecificWorker(proxy_map)\nmyworker.initializing_room()\n\n# The function will be executed within the context of the object,\n# allowing it to modify any attributes or methods associated with it.\n", "description": "" }, "name": "initializing_room", "location": { - "start": 330, - "insert": 333, + "start": 370, + "insert": 373, "offset": " ", "indent": 8, "comment": null @@ -1529,22 +1599,22 @@ "docLength": null }, { - "id": "423155ab-cc3e-9ab0-2342-a93d7e99b214", + "id": "bdadfa6f-3602-06ad-9c4d-0b5443a38eed", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Determines the room name associated with a door based on its ID and updates the graph with new edges and nodes to simulate robot movement through the door and into the adjacent room.", + "description": "Detects the room the robot is currently in and finds the door node leading to the next room. It then creates a new edge in the graph representing the RT pose of the robot and updates the graph with the new edge.", "params": [], "returns": null, "usage": { "language": "python", - "code": "# Initialize a SpecificWorker object with the proxy map\nspecific_worker = SpecificWorker(proxy_map)\n\n# Call the known_room method on the specific worker object\nspecific_worker.known_room()\n", + "code": "# Initialize a SpecificWorker object named \"worker\"\nworker = SpecificWorker(proxy_map)\n\n# Call the known_room method with a given room name and node ID\nworker.known_room(\"room1\", 234)\n", "description": "" }, "name": "known_room", "location": { - "start": 344, - "insert": 346, + "start": 384, + "insert": 386, "offset": " ", "indent": 8, "comment": null @@ -1554,22 +1624,22 @@ "docLength": null }, { - "id": "20438ef8-46d9-6a88-2346-a153824233c9", + "id": "911fde00-668a-68ae-d446-d45ece69baae", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "1) retrieves exit edges and matching doors, 2) sets other side door name and connected room name attributes for each exit door node, and 3) associates doors using their names and connected rooms.", + "description": "1) retrieves edges leading to the exit door, 2) checks if any matching edges exist, and 3) updates node attributes and associates doors based on the matched edges.", "params": [], "returns": null, "usage": { "language": "python", - "code": "SpecificWorker(proxy_map).initializing_doors()\n", - "description": "\nThis code will call the method \"initializing_doors\" of the class SpecificWorker. The method takes no arguments, and it is part of a class that inherits from GenericWorker, so we need to pass an instance of a GenericWorker to the method. In this example, we are passing proxy_map as argument." + "code": "# create a SpecificWorker object\nworker = SpecificWorker(proxy_map)\n\n# initialize the doors in the graph\nworker.initializing_doors()\n", + "description": "" }, "name": "initializing_doors", "location": { - "start": 438, - "insert": 440, + "start": 478, + "insert": 480, "offset": " ", "indent": 8, "comment": null @@ -1579,33 +1649,33 @@ "docLength": null }, { - "id": "f8912fcb-f70e-d0a1-424e-4a48496ff5f8", + "id": "17a38090-fb25-fbbd-6242-3b9b704aaad8", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Connects two doors in a graph by adding an edge between their nodes and updates their attributes to reflect the connection.", + "description": "Connects two nodes representing doors in the long-term graph, adding an edge between them and updating their attributes to reflect the connection.", "params": [ { "name": "door_1", "type_name": "str", - "description": "A list containing the name of the first door to be associated with the long-term graph." + "description": "A list containing the name of the first door to be associated with another door." }, { "name": "door_2", "type_name": "str", - "description": "A name of another door in the graph, which will be connected to the current door node through an edge in the graph." + "description": "A string representation of the second door to be associated with the long-term graph." } ], "returns": null, "usage": { "language": "python", - "code": "worker = SpecificWorker(proxy_map)\ndoor_1 = (\"door_name\", \"connected_room_name\")\ndoor_2 = (\"other_side_door_name\", \"connected_room_name\")\nworker.associate_doors(door_1, door_2)\n", - "description": "\nIn this example, the user is creating a new instance of the SpecificWorker class and then invoking the associate_doors method by passing in two doors (door_1 and door_2). The method will add an edge between the two nodes in the graph representing each door, as well as set the \"other_side_door_name\" and \"connected_room_name\" attributes for both nodes." + "code": "# Initialize the SpecificWorker class object\nworker = SpecificWorker(proxy_map)\n\n# Define two door objects with names and room identifiers\ndoor1 = {\"name\": \"Front Door\", \"room_id\": 2}\ndoor2 = {\"name\": \"Back Door\", \"room_id\": 4}\n\n# Call the associate_doors method on the worker object\nworker.associate_doors(door1, door2)\n", + "description": "" }, "name": "associate_doors", "location": { - "start": 477, - "insert": 479, + "start": 517, + "insert": 519, "offset": " ", "indent": 8, "comment": null @@ -1615,22 +1685,22 @@ "docLength": null }, { - "id": "a8d529d0-b6f1-f692-b541-cfd55b35f825", + "id": "cc21732a-a5cc-7d90-2b4b-65d37df3c117", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Saves the graph representation of a room to a file named \"graph.pkl\" using Python's pickle module.", + "description": "Saves the graph representation of a room node and its exit door id to a file.", "params": [], "returns": null, "usage": { "language": "python", - "code": "# Example usage of store_graph function\nworker = SpecificWorker(proxy_map=ProxyMap(), startup_check=False)\nroom_node_id = -1 # Room node id\nworker.store_graph(room_node_id)\n", - "description": "\nIn this example, the user creates an instance of the `SpecificWorker` class and calls the `store_graph` method with a room node ID as an argument. The function then tries to find the corresponding room node in the igraph object using the `find` method and saves it to a file if it is not found." + "code": "from worker import SpecificWorker\n\n# Initialize the worker\nworker = SpecificWorker(proxy_map, startup_check=False)\n\n# Create a new room node\nroom_node = worker.g.vs.find(name=\"New Room\")\nif not room_node:\n room_node = worker.g.add_vertex(name=\"New Room\")\n\n# Store the graph in pickle format\nwith open(\"graph.pkl\", \"wb\") as f:\n pickle.dump(worker.long_term_graph.g, f)\n", + "description": "" }, "name": "store_graph", "location": { - "start": 496, - "insert": 497, + "start": 536, + "insert": 537, "offset": " ", "indent": 8, "comment": null @@ -1640,22 +1710,22 @@ "docLength": null }, { - "id": "0cdf2d45-fad8-44a8-c74d-90a9b7d7fe2d", + "id": "f099c4de-b784-939e-6846-a148b2aeec31", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Removes edges from the long-term graph based on room numbers and updates the state of the worker.", + "description": "Removes edges from the long-term graph based on certain conditions, including room numbers and types of edges.", "params": [], "returns": null, "usage": { "language": "python", - "code": "# Create an instance of SpecificWorker\nspecific_worker = SpecificWorker(proxy_map)\n\n# Call the removing method with a specific node as its argument\nspecific_worker.removing(200)\n", - "description": "\nIn this example, we create an instance of `SpecificWorker` and then call the `removing` method with a specific node as its argument. The function removes all nodes connected to the specified node by any edge with the \"RT\" type. It also removes all edges of the \"has\" type that have the specified node as their origin or destination, and deletes the specified node." + "code": "specific_worker = SpecificWorker(proxy_map, startup_check=True)\nspecific_worker.removing()\n", + "description": "\nThis code creates a new instance of the `SpecificWorker` class and then calls the `removing` method on it. The `removing` function is defined in the `SpecificWorker` class, and it removes certain elements from the graph using the `delete_node` method. The graph is an attribute of the `SpecificWorker` class, and it represents a long-term spatial memory for a robot named Shadow. The `startup_check` argument passed to the `SpecificWorker` constructor is set to `True`, indicating that the startup check should be performed before the removal process can begin." }, "name": "removing", "location": { - "start": 510, - "insert": 512, + "start": 550, + "insert": 552, "offset": " ", "indent": 8, "comment": null @@ -1665,28 +1735,28 @@ "docLength": null }, { - "id": "24746b75-48b7-4bb6-1b42-dbcab50aabd8", + "id": "18553e7e-c150-41ac-6742-b52ca312d465", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Traverses a graph by starting at a designated node and following RT edges to reach other nodes, inserting vertices and edges into an IGraph object as it goes.", + "description": "Traverses a graph and performs actions based on the type of edges it encounters. It starts at a designated node, then visits the children nodes of that node according to the RT edge type, and repeats this process for each child node until no more actions are left to perform.", "params": [ { "name": "node_id", "type_name": "int", - "description": "Used to represent the unique identifier for a node in the graph." + "description": "Represented by the igraph vertex ID of the current node being traversed." } ], "returns": null, "usage": { "language": "python", - "code": "# Create a SpecificWorker instance with proxy_map and startup_check arguments set to True\nspecific_worker = SpecificWorker(proxy_map, startup_check=True)\n\n# Traverse the graph starting from the robot node\nspecific_worker.traverse_graph(\"Robot\")\n", - "description": "\nThis example uses the traverse_graph function of the SpecificWorker class to traverse a graph starting from the \"Robot\" node." + "code": "specific_worker = SpecificWorker(proxy_map, startup_check=False)\nnode_id = \"a_node_id\"\nspecific_worker.traverse_graph(node_id)\n", + "description": "\nThis example shows how the traverse_graph method of the SpecificWorker class can be used to start traversing a graph from a specific node. The node ID is passed as an argument to the function, and the function then performs the necessary steps to traverse the graph starting at that node." }, "name": "traverse_graph", "location": { - "start": 536, - "insert": 538, + "start": 576, + "insert": 578, "offset": " ", "indent": 8, "comment": null @@ -1696,28 +1766,28 @@ "docLength": null }, { - "id": "500a6fa2-405e-aa91-9540-a1c44ea71e8e", + "id": "8ac6a6a7-f160-089e-bb4c-9b09f663081c", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Traverses the graph by visiting all vertices that have successors in the long-term graph, and for each such vertex, it checks if its successor has a higher level than the current vertex, and if so, it inserts a DSR vertex and edge between them, and recursively traverses the successor.", + "description": "Traverses the graph, identifying vertices with higher level than the current vertex and adding them to the DSR graph along with their connections.", "params": [ { "name": "node", "type_name": "igraph.Vertex", - "description": "Used to represent the current node being traversed." + "description": "Representing a vertex in the graph." } ], "returns": null, "usage": { "language": "python", - "code": "graph = SpecificWorker(proxy_map)\nstarting_node = graph.g.get_node(\"MyStartingNode\")\ngraph.traverse_igraph(starting_node)\n", - "description": "\nThe code creates an instance of the SpecificWorker class and passes a proxy map object to the constructor. Then, it retrieves a starting node from the graph using the g.get_node method and uses this node as the argument for the traverse_igraph function. The function then recursively traverses the graph, inserting DSR vertices and edges based on the condition in the if statement." + "code": "specific_worker = SpecificWorker(proxy_map)\nspecific_worker.traverse_igraph(specific_worker.g.vs[0])\n", + "description": "\nThis code creates a new object of type SpecificWorker and then calls the traverse_igraph method, passing in the first vertex of the graph as an argument. This vertex is used to start the traversal of the graph." }, "name": "traverse_igraph", "location": { - "start": 546, - "insert": 547, + "start": 586, + "insert": 587, "offset": " ", "indent": 8, "comment": null @@ -1727,28 +1797,28 @@ "docLength": null }, { - "id": "60d7e0f3-5642-a39a-4e40-66246d4c4948", + "id": "8d4576e6-5bd3-6d9d-6548-08acb2abc696", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Adds a new vertex to an existing graph, updating its attributes and edges based on node-specific data.", + "description": "Adds a new vertex to an existing graph, updating the graph's vertices with provided attributes and matching other vertices based on specified conditions.", "params": [ { "name": "node", - "type_name": "igraph.Node", - "description": "Used to insert a new vertex into the graph." + "type_name": "igraph.Vertex", + "description": "Used to represent a single vertex in the graph, including its name, ID, and attributes." } ], "returns": null, "usage": { "language": "python", - "code": "# create a new instance of the SpecificWorker class and pass in proxy_map as argument\nspecific_worker = SpecificWorker(proxy_map)\n\n# add a vertex to the long term graph with the name \"MyVertex\"\nspecific_worker.insert_igraph_vertex(\"MyVertex\")\n", - "description": "\nIn this example, we first create a new instance of the SpecificWorker class and pass in proxy_map as an argument. We then use the insert_igraph_vertex function to add a vertex with the name \"MyVertex\" to the long term graph." + "code": "worker = SpecificWorker() # Initiate the class\nnode = Node(name=\"Door\", id=1, type=\"door\") # Create a node with name and ID\nworker.insert_igraph_vertex(node) # Pass the node to the function\n", + "description": "\nThe example shows how a node with name \"Door\" and ID \"1\" is created and then passed to the insert_igraph_vertex() function, which will add it as a vertex in the graph. The function also takes care of any other attributes that may be present in the node object, such as parent or timestamp creation." }, "name": "insert_igraph_vertex", "location": { - "start": 559, - "insert": 560, + "start": 599, + "insert": 600, "offset": " ", "indent": 8, "comment": null @@ -1758,33 +1828,33 @@ "docLength": null }, { - "id": "7467a4df-2da4-a89d-dc40-8d0db7be370f", + "id": "705003db-9448-d6bc-2145-c9b70fa2a402", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Inserts a new node into a graph, updating the parent node's attribute with the agent ID and copying over non-required attributes from the input node to the new node.", + "description": "Modifies an existing node within a graph, adding new attributes and linking it to its parent node through an attribute.", "params": [ { "name": "parent_name", "type_name": "str", - "description": "Used to specify the name of the parent node to which the new node will be added." + "description": "Used to specify the name of the parent node to insert the new vertex into." }, { "name": "node", "type_name": "Node | dict", - "description": "Passed the attributes and data of a vertex to be inserted into a graph." + "description": "Passed in from caller, it contains attributes that define the vertex to be inserted into graph." } ], "returns": null, "usage": { "language": "python", - "code": "# Create a new instance of SpecificWorker class and call its constructor\nmy_worker = SpecificWorker(proxy_map)\n\n# Use the worker to add a vertex to the graph\nmy_worker.insert_dsr_vertex(\"parent_node\", {\"type\": \"room\", \"name\": \"Living Room\"})\n", - "description": "" + "code": "specific_worker = SpecificWorker(proxy_map)\nspecific_worker.insert_dsr_vertex(\"parent_name\", {\"type\": \"node_type\", \"name\": \"node_name\"})\n", + "description": "\nThis example inserts a node with the name \"node_name\" into the graph with the type \"node_type\" and links it to the parent node named \"parent_name\"." }, "name": "insert_dsr_vertex", "location": { - "start": 594, - "insert": 596, + "start": 634, + "insert": 636, "offset": " ", "indent": 8, "comment": null @@ -1794,28 +1864,28 @@ "docLength": null }, { - "id": "ac75a866-f308-2a90-484f-864828cce9c4", + "id": "57a732e1-901a-ef80-944c-23f4f0907fda", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Adds an edge to a long-term graph based on a given edge attribute, updating the node positions and rotation accordingly.", + "description": "Adds an edge to a long-term graph based on a received edge attribute. It checks if the destination node is the robot, and if so, uses the correct translation and rotation attributes. Otherwise, it uses the provided translation and rotation attributes.", "params": [ { "name": "edge", - "type_name": "rt.Edge", - "description": "Passed by reference to the method, representing an edge with attributes such as translation and rotation." + "type_name": "GraphEdge", + "description": "Passed as a whole object containing information about an edge in the graph, including its origin and destination nodes, as well as attribute values such as translation and rotation." } ], "returns": null, "usage": { "language": "python", - "code": "# Create an instance of SpecificWorker\nmy_worker = SpecificWorker()\n\n# Create an edge object with attributes \"origin\" and \"destination\"\nedge = Edge(origin=\"node1\", destination=\"node2\")\n\n# Call the insert_igraph_edge method on my_worker with the edge as an argument\nmy_worker.insert_igraph_edge(edge)\n", - "description": "\nIn this example, an instance of SpecificWorker is created and an Edge object is created with attributes \"origin\" and \"destination\". The insert_igraph_edge method of SpecificWorker is then called with the Edge object as an argument, which adds a new edge to the graph." + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nedge = Edge(\"Origin\", \"Destination\")\nedge.attrs[\"rt_translation\"] = [10, 20, 30]\nedge.attrs[\"rt_rotation_euler_xyz\"] = [45, 90, 180]\nworker.insert_igraph_edge(edge)\n", + "description": "\nIn this example, a user creates an instance of the SpecificWorker class and then adds an edge to the graph by calling the insert_igraph_edge method with the edge object as an argument. The edge object contains information about the origin and destination nodes of the edge, such as their names or IDs. The method then uses this information to add a new edge to the long-term spatial memory graph with the given translation and rotation attributes." }, "name": "insert_igraph_edge", "location": { - "start": 608, - "insert": 609, + "start": 648, + "insert": 649, "offset": " ", "indent": 8, "comment": null @@ -1825,33 +1895,33 @@ "docLength": null }, { - "id": "35a1d65c-21d4-f7be-c446-071ce717b9a2", + "id": "3f1ca468-3163-3ba8-e54f-a2fb9ce810fa", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Modifies an existing graph by adding an edge with specific RT attributes, based on input org and dest nodes.", + "description": "Creates a new edge in the long-term graph based on data from the short-term graph. It assigns attributes to the new edge, including a translation and rotation value for RT tracking.", "params": [ { "name": "org", "type_name": "GraphNode | NoneType", - "description": "Used to represent the origin node of the edge to be inserted. If it is None, the function will use the root node as the origin." + "description": "Used to specify the starting node of the edge to be inserted. If it is None, it means the starting node is unknown or unspecified." }, { "name": "dest", "type_name": "Node | str", - "description": "Used to specify the destination node or name in the graph for insertion of an edge." + "description": "Used to specify the destination node or edge id for the edge insertion operation." } ], "returns": null, "usage": { "language": "python", - "code": "worker = SpecificWorker(proxy_map)\n# Create a new edge between two nodes in the graph\nworker.insert_dsr_edge(\"node1\", \"node2\")\n", - "description": "\nIn this example, the insert_dsr_edge function is called on an instance of the SpecificWorker class (which inherits from GenericWorker). The function takes in two arguments: org and dest. Org represents the origin node of the edge, and dest represents the destination node. The function first checks if org is None. If it is not None, then it retrieves the edge ID between org and dest using g.get_eid() and retrieves the attributes of the edge using g.es[edge_id]. It then creates a new Edge object with the destination node ID as its origin, the origin node ID as its destination, and an RT attribute with a value of 0 (as this is not a realistic use case). It then assigns the RT translation and rotation attributes to the new edge using new_edge.attrs[\"rt_translation\"] = Attribute(np.array([0, 0, 0], dtype=np.float32), self.agent_id) and new_edge.attrs[\"rt_rotation_euler_xyz\"] = Attribute(np.array([0, 0, 0], dtype=np.float32), self.agent_id). Finally, it inserts the new edge into the graph using g.insert_or_assign_edge()." + "code": "worker = SpecificWorker(proxy_map)\n# Define the nodes and edges in the graph\nnode1 = Node(\"Node 1\", agent_id=worker.agent_id)\nnode2 = Node(\"Node 2\", agent_id=worker.agent_id)\nedge = Edge(node1, node2, \"RT\", worker.agent_id)\n# Call the function to insert the edge into the graph\nworker.insert_dsr_edge(node1, node2)\n", + "description": "" }, "name": "insert_dsr_edge", "location": { - "start": 625, - "insert": 628, + "start": 665, + "insert": 668, "offset": " ", "indent": 8, "comment": null @@ -1861,31 +1931,31 @@ "docLength": null }, { - "id": "d927aae0-9021-6196-9b41-95f639864b45", + "id": "04491164-d053-7abe-e94a-0772c85a56b6", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Determines the room number of an element by retrieving the attribute 'room_id' from the element's node ID.", + "description": "Determines the room number associated with a given node ID by retrieving the attribute \"room_id\" from the node and returning its value if found, or -1 otherwise.", "params": [ { "name": "node_id", - "type_name": "str", - "description": "Used to reference a node in the graph." + "type_name": "int", + "description": "Passed as an argument to the function, indicating the id of the element whose room number needs to be checked." } ], "returns": { "type_name": "int", - "description": "The room ID associated with a given node ID." + "description": "The room ID associated with a given node ID if the node has an attribute \"room_id\" and its value is not None, or -1 otherwise." }, "usage": { "language": "python", - "code": "# Initialize the SpecificWorker class and its attributes\nspecific_worker = SpecificWorker()\n\n# Get the room number of a node with ID \"my_node\"\nroom_id = specific_worker.check_element_room_number(\"my_node\")\nprint(f\"The room number of node 'my_node' is {room_id}\")\n", + "code": "worker = SpecificWorker(proxy_map)\nnode_id = 1234567890\nroom_id = worker.check_element_room_number(node_id)\nif room_id == -1:\n print(\"Node is not in a room\")\nelse:\n print(\"Node is in room\", room_id)\n", "description": "" }, "name": "check_element_room_number", "location": { - "start": 655, - "insert": 656, + "start": 695, + "insert": 696, "offset": " ", "indent": 8, "comment": null @@ -1895,31 +1965,31 @@ "docLength": null }, { - "id": "5ce9e553-2b0d-88b8-bf49-7882013c2d15", + "id": "f912f9d3-d6ed-b482-ee40-a7c750e382d7", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Determines the element level of a node based on its attribute \"level\" and returns it if found, or -1 otherwise.", + "description": "1) retrieves the element level of a node with the given ID, and 2) handles missing or non-existent element levels by printing an error message and returning -1.", "params": [ { "name": "node_id", "type_name": "int", - "description": "Represented by the identifier `node_id`. It serves as an index into the graph's node list to access the specified node." + "description": "Representing an element node identifier, which identifies a specific element in the graph." } ], "returns": { "type_name": "int", - "description": "Element level of the node with given id" + "description": "Element level if found, or -1 otherwise." }, "usage": { "language": "python", - "code": "# Import SpecificWorker class from GenericWorker module\nfrom GenericWorker import SpecificWorker\n\n# Initialize a new SpecificWorker object\nmy_specific_worker = SpecificWorker(\"proxy_map\")\n\n# Call the check_element_level method with the desired node ID as an argument\nelement_level = my_specific_worker.check_element_level(1234)\n\n# Print the element level value to the console\nprint(f\"Element level of node 1234 is {element_level}\")\n", + "code": "# Initialize SpecificWorker class instance\nworker = SpecificWorker(proxy_map)\n\n# Get node_id of a specific element in the graph\nnode_id = 123456789\n\n# Call check_element_level function to retrieve level attribute value from node\nelement_level = worker.check_element_level(node_id)\n\n# Print retrieved level attribute value for the specified element\nprint(\"Element level:\", element_level)\n", "description": "" }, "name": "check_element_level", "location": { - "start": 664, - "insert": 665, + "start": 704, + "insert": 705, "offset": " ", "indent": 8, "comment": null @@ -1929,28 +1999,28 @@ "docLength": null }, { - "id": "ca797b1f-3a72-7887-b54c-bcac0d9c7872", + "id": "1141fbc6-886b-ab8c-324d-576e1b3fb114", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "1) retrieves room information from the graph, 2) identifies RT edges connecting to the current room, and 3) extracts translation attribute for the current room.", + "description": "Retrieves and processes information about a room node in a graph, including its ID, old RT edges, translation attribute, and doors. It then draws the room polygon and doors on an image for saving.", "params": [ { "name": "room_node_id", "type_name": "str | int", - "description": "Required, indicating that it is mandatory to pass this value for the function to work correctly." + "description": "Used to identify the room for which the picture will be generated." } ], "returns": null, "usage": { "language": "python", - "code": "# Creating a specific worker object\nspecific_worker = SpecificWorker(proxy_map)\n\n# Generating a room picture for a specific room node id\nspecific_worker.generate_room_picture(123456789)\n\n# Saving the image to a file\ncv2.imwrite(\"room_image.jpg\", specific_worker.room_image)\n", + "code": "specific_worker = SpecificWorker(proxy_map)\n# Startup check and initialize the robot's name, id, and last robot pose.\nspecific_worker.startup_check()\n\n# Generate the room picture using the specific worker instance and the desired room node ID.\nroom_node_id = 123456 # Replace with actual node ID\nspecific_worker.generate_room_picture(room_node_id)\n", "description": "" }, "name": "generate_room_picture", "location": { - "start": 707, - "insert": 709, + "start": 747, + "insert": 749, "offset": " ", "indent": 8, "comment": null @@ -1960,28 +2030,28 @@ "docLength": null }, { - "id": "9d6cc73f-8cb4-e89e-7945-94e9bd98c8ac", + "id": "abb52b39-2b77-1e95-5943-2ff31d0cfa5b", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Inserts or assigns an edge in the graph representing the room hierarchy, with the current worker's ID as the source and the current room ID as the target.", + "description": "Inserts or assigns an edge in the graph with the current room ID as the tail and the same ID as the head, indicating that the agent is currently located in that room.", "params": [ { "name": "room_id", "type_name": "str", - "description": "Used as an identifier for the current room that the agent is located in." + "description": "Used to represent the id of the current room in which the agent is located." } ], "returns": null, "usage": { "language": "python", - "code": "def insert_current_edge(self, room_id):\n current_edge = Edge(room_id, room_id, \"current\", self.agent_id)\n self.g.insert_or_assign_edge(current_edge)\n", - "description": "\nIn this example, the SpecificWorker class is used to insert an edge into a graph representing a LongTermSpatialMemory with the robot's current location represented by room_id. The function first creates an Edge object with the same room_id as the target and source node ids. Then, it inserts or assigns the created edge into the graph using the insert_or_assign_edge() method of the DSRGraph class." + "code": "worker = SpecificWorker(proxy_map)\nworker.insert_current_edge(room_id)\n", + "description": "" }, "name": "insert_current_edge", "location": { - "start": 742, - "insert": 744, + "start": 782, + "insert": 784, "offset": " ", "indent": 8, "comment": null @@ -1991,33 +2061,33 @@ "docLength": null }, { - "id": "339e852e-5da6-aa92-6146-a2388fe634c4", + "id": "c580b131-a2f7-48b2-d347-fd8315f9d58a", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Updates a node in the graph based on its ID and type. If the type is \"door\", it inserts a vertex in the long-term graph, checks if a room node exists, and inserts an edge with a specific translation and rotation. If the ID is the affordance node active ID, it checks the state and activeness of the node and transitions to the crossed state.", + "description": "Updates the node attributes based on user input and inserts new edges into the graph according to predefined rules.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to identify the node being updated." + "description": "Used to identify the node to be updated." }, { "name": "type", "type_name": "str", - "description": "Used to identify the node being updated, with possible values of \"door\" or \"room\"." + "description": "Used to identify the type of node being updated (door or room)." } ], "returns": null, "usage": { "language": "python", - "code": "# Create a specific worker object and initialize it with the startup check flag set to True\nspecific_worker = SpecificWorker(proxy_map, startup_check=True)\n\n# Call the update_node method of the specific worker object with the node ID and type as arguments\nspecific_worker.update_node(1234567890, \"door\")\n", - "description": "\nThis code creates a SpecificWorker object and initializes it with the startup check flag set to True. Then it calls the update_node method of the specific worker object with the node ID 1234567890 and type \"door\" as arguments. The function would then check if there is an igraph vertex for the door node, insert it if necessary, and then check if there are any edges between the parent node and the door node in the long-term graph, inserting them if necessary and drawing the updated graph." + "code": "# Call the function update_node() with parameters\nworker.update_node(1, \"door\")\n", + "description": "\nIn this code snippet, we call the `update_node()` function in the `SpecificWorker` class by passing two arguments: 1 and \"door\". The first argument is the id of the node to be updated, and the second argument is the type of the node, which is door in this case. This call will update the node with the id 1 with the new type information.\n\nWe can also use `update_node()` to update other types of nodes, such as rooms or affordances, by passing different arguments for the type parameter. For example:\n" }, "name": "update_node", "location": { - "start": 756, - "insert": 757, + "start": 796, + "insert": 797, "offset": " ", "indent": 8, "comment": null @@ -2027,38 +2097,38 @@ "docLength": null }, { - "id": "2abe3cea-a1da-b5bc-224c-fb099b8b8257", + "id": "1b073fde-9b9f-83a1-3541-533c6e7e206e", "ancestors": [ - "8012c732-b5dd-93a9-2441-d47f31f3d2fa" + "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" ], - "description": "Updates the current edge based on the specified frame, to node, and type. If no current edge exists, it inserts a new edge and sets it as the current one.", + "description": "Updates an edge in the graph based on its type, source node, and target node.", "params": [ { "name": "fr", "type_name": "int", - "description": "Representative of the 'from' node value, which indicates the starting point of an edge" + "description": "Representing the first room number of an edge to be updated with new information." }, { "name": "to", "type_name": "int", - "description": "The target node ID of an edge to be updated." + "description": "Used to identify the target node in the graph for updating the edge." }, { "name": "type", "type_name": "str", - "description": "Used to specify the type of edge being updated, either \"RT\" for real-time or \"NRT\" for non-real-time." + "description": "Used to indicate whether the edge is a robot or a room node." } ], "returns": null, "usage": { "language": "python", - "code": "worker = SpecificWorker(proxy_map)\n# Initialize the worker with the given proxy map and startup check set to False.\n\nworker.update_edge(fr=10, to=20, type=\"RT\")\n# Update the edge connecting nodes 10 and 20 with type \"RT\".\n", - "description": "" + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\n# Update edge in graph\nworker.update_edge(fr=1, to=2, type=\"RT\")\n", + "description": "\nThis updates the edge with fr as the starting node and to as the ending node with the type of \"RT\"." }, "name": "update_edge", "location": { - "start": 811, - "insert": 815, + "start": 851, + "insert": 855, "offset": " ", "indent": 8, "comment": null diff --git a/.komment/komment.json b/.komment/komment.json index fde23142..822afa63 100644 --- a/.komment/komment.json +++ b/.komment/komment.json @@ -1,7 +1,7 @@ { "meta": { "version": "1", - "updated_at": "2024-07-15T13:16:52.559Z", + "updated_at": "2024-07-17T07:58:01.192Z", "created_at": "2024-07-03T15:48:27.349Z", "pipelines": [ "50c96b7c-ad3b-4b93-ae19-7f6fbd5637f7", @@ -10,7 +10,8 @@ "1f636eb0-7f1e-4d1d-b462-6d1eff3533cb", "d6a72e5c-096b-4179-ac39-3343ea5ec3a5", "395b32ba-01b1-45bb-84e0-96a1b0abb44e", - "fb0c09b8-650c-42fb-a814-0c07d121ec67" + "fb0c09b8-650c-42fb-a814-0c07d121ec67", + "3216d745-95dc-419d-8524-71b95ba17b45" ] }, "lookup": [ diff --git a/agents/g2o_agent/src/specificworker.py b/agents/g2o_agent/src/specificworker.py index ad616c9f..ae801187 100644 --- a/agents/g2o_agent/src/specificworker.py +++ b/agents/g2o_agent/src/specificworker.py @@ -49,129 +49,129 @@ class SpecificWorker(GenericWorker): """ - Manages a specific robot's state and updates its graph, edges, and attributes - based on messages from the occupancy grid. It also handles room changes and - RT sets for the specified robot. + Manages a specific worker's tasks and updates in a graph. It has methods for + updating node attributes, deleting nodes, and setting up RT transmission, as + well as checking for updates and handling errors. Attributes: - Period (float|int): 200 by default, indicating the time interval (in - milliseconds) between updates of the graph. It can be modified to - adjust the update rate of the worker. - agent_id (int|str): A unique identifier for the agent, which can be used - to identify specific agents within the system. - g (Graph|QGraphicsScene): Used to store and manipulate the graph data structure. - startup_check (QTimersingleShot200,QApplicationinstancequit): Set to call - the quit function of the QApplication instance after a delay of 200 milliseconds. - rt_api (Union[int,str]): Used to store the API for the Robot Translation - (RT) functionality in the worker. It can take the value of either an - integer representing the RT algorithm or a string representing the RT - algorithm name. - inner_api (Dict[str,Any]): Used to store additional internal data for the - worker, such as the current room ID or the first RT set. It is only - accessible within the worker's implementation and is not part of the - public API. + Period (float|int): Used to control the update rate of the graph. It + determines how often the graph is updated, with a higher value resulting + in more frequent updates. + agent_id (int|str): Used to store the ID of the agent that will be using + the specific worker. + g (Graph|QGraphicsScene): Used to store the graph data for the worker's + algorithm, including the nodes, edges, and their attributes. + startup_check (QTimersingleShot200,QApplicationinstancequit): Used to check + if the application is still running after a certain time period (200 + milliseconds) and quit if so. + rt_api (Union[str,int]): Used to store the RT (Real-time) edge set ID for + the worker's specific room. + inner_api (Callable[[],None]): Used to define a custom API for the worker + to interact with its internal data structures and algorithms without + exposing them to the outer world. odometry_node_id (int): Used to identify the node in the graph that - represents the robot's position and orientation. - odometry_queue (List[Tuple[float,float,float,int]]): Used to store the - latest odometry data from the robot's sensors for graph updating. + represents the robot's odometry information. + odometry_queue (List[Tuple[float,float,float,int]]): Used to store odometry + data from the environment, containing the robot's current advance + speed, side speed, and angular speed, as well as the timestamp in + milliseconds since the epoch. last_odometry (float|List[float]): Used to store the last known odometry - information of the robot, including its position, velocity, and angular - velocity, which are updated every 200 milliseconds. - g2o (Graph|npndarray): Used to represent the graph of the environment, - allowing the worker to perform operations on it such as adding or - deleting nodes and edges. - odometry_noise_std_dev (float|int): Used to represent the standard deviation - of noise in the odometry measurements. It determines how much the - actual robot position, orientation, and velocity are expected to deviate - from their predicted values based on the odometry measurements. - odometry_noise_angle_std_dev (float|int): 1.0 by default, which represents - the standard deviation of the noise angle in the odometry measurement. - It helps to quantify the uncertainty in the estimated angle of the - robot's movement. - measurement_noise_std_dev (float|int): Used to represent the standard - deviation of measurement noise in the robot's odometry readings. It - is used to estimate the uncertainty of the robot's position and velocity. - last_room_id (str|int): Used to store the last room ID seen by the agent - before changing rooms, used for updating the RT set. - actual_room_id (str|int): Used to store the current room ID of the agent, + values (position, orientation, and translation) of the robot, which + are updated in real-time during the simulation. + g2o (Graph|Tuple[str,str]): Used to represent the graph structure of the + robot's environment. It contains a tuple of two elements: the first + element is the name of the graph file, and the second element is the + name of the node file. + visualizer (Callable[[QWidget],None]): Used to create a QWidget instance + for visualizing the robot's movement and orientation in real-time. + odometry_noise_std_dev (float|int): 0.1 by default, representing the + standard deviation of noise added to the odometry measurements for the + robot's position, orientation, and angular velocity. + odometry_noise_angle_std_dev (float|double): 0.2 by default, representing + the standard deviation of angle noise in the odometry estimates. It + affects how much the worker's estimate of the agent's position and + orientation deviates from the true values due to random noise in the + sensor readings. + measurement_noise_std_dev (float|int): 0.1 by default, indicating the + standard deviation of the noise present in the robot's measurements. + last_room_id (int|str): Used to store the room ID of the previous room + that the agent has entered before changing rooms, which is useful for + determining when the agent has moved to a new room. + actual_room_id (int|str): Used to store the current room ID of the agent, which is updated when the agent moves to a new room. - elapsed (float|int): Used to keep track of the time since the last call - to the `update()` method, which is used to control the frequency of - updates to the graph. + elapsed (float|int): Used to store the time since the last call to the + `update()` method, which is used to update the node's attributes. room_initialized (bool): Set to False when a room change is detected, - indicating that the worker has switched rooms. It is then reset to - True once the new room's ID is determined. - iterations (int|List[int]): Used to keep track of the number of iterations - of the worker's function that have been performed, or the list of - iteration numbers if the function is called multiple times with different - inputs. - hide (bool): Used to hide the worker from the graphical user interface - (GUI) when set to True. - init_graph (bool): Used to track whether the graph has been initialized - by the worker during its lifetime. It is used to prevent unnecessary - work from being performed when the graph has already been initialized. - current_edge_set (bool): Used to indicate whether the current edge being - processed is part of a RT set or not. It is set to True when a new RT - edge is encountered, and False otherwise. - first_rt_set (bool): Set to True when the robot performs its first RT - (Real-time) movement, indicating that the worker has started tracking - RT movements. - translation_to_set (float|List[float]): Used to store the translation value - to set for the shadow robot. The list stores the values of the translation - in the x, y, and z dimensions respectively. - rotation_to_set (float|int): Used to store the rotation of the robot to - set its position relative to its starting point. - room_polygon (Tuple[float,float,float,]): Used to store the coordinates - of a room's polygon in a graph. - security_polygon (Polygon|List[Point]): Used to store the security polygon - of a room, which is used for collision detection and avoidance during - robot navigation. - initialize_g2o_graph (void|QTimersingleShot200,QApplicationinstancequit): - Used to initialize a Graph2O graph for representing the environment. - rt_set_last_time (int|float): Used to track the time since the last RT - (Room To) set by the agent. It is used to determine when to reset the - translation and rotation to set values. - rt_time_min (float|int): Defined as a minimum time gap between two successive - RT edge sets, used to determine when to update the translation and - rotation values for the agent. - last_update_with_corners (int|bool): Used to keep track of when the worker - has last updated its state with corners data. It is initially set to - True, indicating that the worker has not yet received any corners data, - and then is updated to False whenever the worker receives new corners - data. - timer (int|float): Used to track the time elapsed since the last update - of the graph, with the purpose of checking if it's been a certain - amount of time (200ms) since the last update, and if so, quit the application. - compute (Callable[[float,float],float]): Used to compute the next node ID - for the worker's node. It takes two arguments: the current time and a - seed value, and returns the next node ID as a float value. - update_node_att (Tuple[int,str,int,int]): Used to update the attributes - of a node in the graph when the node's ID matches the given ID. The - attribute takes four arguments: the node ID, the attribute names, the - old value, and the new value. - update_edge (Callable[float,float,str]): Used to update the edge attributes - of a graph based on specific conditions. It takes three arguments: the - first is the source node ID, the second is the target node ID, and the - third is the edge type. The attribute is called whenever a new edge - is added or an existing edge is modified in the graph, and it can be - used to set or update edge attributes based on specific conditions. - update_edge_att (List[str]): Used to update the attributes of an edge based - on a specific type and name. It takes three parameters: the first is - the id of the edge, the second is the type of edge, and the third is - a list of attribute names to be updated. + indicating that the worker has not yet initialized its current room's + graph. It is reset to True once the worker has successfully initialized + the new room's graph. + iterations (int|float): Used to keep track of the number of iterations + (i.e., frames) that the worker has processed. It is updated with each + frame processed by the worker, and can be used to control the speed + of the worker or to implement various algorithms. + hide (bool): Used to indicate whether the worker should be hidden or not, + affecting its visibility in the graph. + init_graph (bool): Used to indicate whether the graph has been initialized + or not. It is set to True when the worker is initialized and False + otherwise, allowing for proper handling of the graph and its nodes and + edges in the update methods. + current_edge_set (bool): Used to track whether an edge set has been computed + for the current room id. It is set to True when the first RT edge set + is computed and False otherwise, allowing the worker to avoid recomputing + the edge set if it has already been computed for the same room id. + first_rt_set (bool): Set to `True` when the RT translation and rotation + values are being set for the first time, and `False` otherwise. + translation_to_set (float|List[float]): Used to store the translation + values set by the RT algorithm. + rotation_to_set (float|int): A value representing the rotation of the robot + to set its end effector at a specific location. + room_polygon (List[float]): Used to store the vertices of a polygon that + represents the room boundary in the environment. + security_polygon (Union[int,List[float]]): Used to store the security + polygon of the worker's workspace in 3D space. It is used to detect + collisions between the worker's manipulator and objects in the environment. + initialize_g2o_graph (void): Used to create a new graph for the given + worker instance, which will be used to store the robot's position and + orientation in the environment. + rt_set_last_time (int|float): Used to store the last time a RT (Real-Time) + edge was set for a specific agent ID. It is used in conjunction with + the `rt_time_min` attribute to determine when to reset the RT translation + and rotation values. + rt_time_min (float): Defined as the minimum time duration between two + consecutive RT sets in milliseconds. It is used to determine when to + reset the translation and rotation values for the shadow agent during + RT tracking. + last_update_with_corners (int|bool): Used to keep track of the last time + a node or edge update was performed with corner information. It is + initially set to False, and its value changes when updates are performed + with corners. Its purpose is to check if the worker has updated with + corners before, so as to avoid unnecessary updates in the future. + timer (float|int): Used to schedule a call to the `QApplication.instance().quit()` + function after 2 seconds. + compute (Callable[[],Any]): Used to define a function that computes the + worker's task based on the graph and other attributes. + update_node_att (Callable[int,[str]]): Used to update the attributes of a + specific node in the graph based on its ID and the type of update. It + takes two arguments: id, which is the ID of the node to be updated, + and attribute_names, which is a list of attribute names to be updated. + update_edge (Callable[int,str,str]): Used to update the attributes of an + edge in the graph based on its type (current or RT translation) and + the node it connects. + update_edge_att (Tuple[str,]): Used to update the attributes of an edge + in a graph based on its type and the name of the attribute. """ def __init__(self, proxy_map, startup_check=False): """ - Initializes the worker's internal state, including its graph, odometry - queue, and other components essential for its operation. + Initializes instance variables and sets up event handling for signals + related to node and edge attributes, as well as a timer to compute the + worker's output every `Period` seconds. Args: - proxy_map (Dict[str, Any]): Used to pass additional data to the - SpecificWorker object. - startup_check (bool): Used to check if the graph has been properly - initialized before starting the worker's computation. If set to - `False`, it will skip this check and proceed with the computation. + proxy_map (Dict[str, Any]): Used to store the mapping between the + original node attributes and their proxies. + startup_check (bool): Used to check if the graph has already been + initialized before starting the worker's execution. """ super(SpecificWorker, self).__init__(proxy_map) @@ -192,7 +192,7 @@ def __init__(self, proxy_map, startup_check=False): self.last_odometry = None # Initialize g2o graph with visualizer self.g2o = G2OGraph(verbose=False) - # self.visualizer = G2OVisualizer("G2O Graph") + self.visualizer = G2OVisualizer("G2O Graph") self.odometry_noise_std_dev = 1 # Standard deviation for odometry noise self.odometry_noise_angle_std_dev = 1 # Standard deviation for odometry noise @@ -248,8 +248,8 @@ def setParams(self, params): @QtCore.Slot() def compute(self): """ - Processes robot odometry data and updates the graph using G2O optimization - to estimate the robot's position and orientation. + Computes and updates the robot's position, orientation, and covariance + matrix based on RT messages received from the environment. """ if time.time() - self.elapsed > 1: @@ -341,7 +341,7 @@ def compute(self): # 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) + self.visualizer.update_graph(self.g2o) # 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] @@ -364,8 +364,9 @@ def compute(self): 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: + 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") @@ -383,13 +384,14 @@ def initialize_g2o_graph(self): # print("Initializing g2o graph") # get robot pose in room """ - 1) initializes the g2o graph based on room and robot nodes, 2) adds nominal - corners for rooms and doors, and 3) fixes the pose of a robot node using - odometry information. + Initializes a Graph-Based Odometry (G2O) graph for a specific room by: + 1/ Retrieving node and edge information from a robot's sensor data. + 2/ Filtering out invalid nodes and edges based on their position and orientation. + 3/ Adding nominal corners and fixed poses to the G2O graph using the + retrieved data. Returns: - bool: 1 if the g2o graph is successfully initialized with nominal - corners and fixed poses, and 0 otherwise. + bool: True if the g2o graph is successfully initialized and False otherwise. """ self.g2o.clear_graph() @@ -596,18 +598,18 @@ def initialize_g2o_graph(self): def get_displacement(self, odometry): """ - Calculates the displacement of the robot in three dimensions (lateral, - forward, and angular) based on the odometry data stored in a queue. + Computes the displacement of a robot based on its odometry data, taking + into account the advance, lateral movement, and angular velocity of the robot. Args: - odometry (Tuple[float, float, float]): A sequence of 3-element tuples - representing the robot's position, velocity, and timestamp in a - time interval. + odometry (Tuple[float, float, float]): An immutable sequence containing + the robot's odometry data at each time step, consisting of the + linear displacement, lateral displacement, and angular velocity, + respectively. Returns: - Tuple[float,float,float]: 3 floating-point values representing the - lateral displacement, forward displacement, and angular displacement - of an object over a given time interval. + Tuple[float,float,float]: The displacement along the x, y and z axes, + respectively, calculated using the odometry data from a robot's sensors. """ desplazamiento_avance = 0 @@ -630,18 +632,17 @@ def get_displacement(self, odometry): def get_covariance_matrix(self, vertex): """ - Computes the covariance matrix between a given vertex and all other vertices - in the graph, using the G2O optimizer to compute marginals and upper - triangle matrix representation. + Computes the covariance matrix of a set of vertices in a graph, using an + optimization algorithm to compute the marginals of the vertices and then + constructing the covariance matrix from the resulting upper triangle. Args: vertex (G2O.Vertex | G2O.HessianIndex): Used to compute the covariance - matrix for a specific vertex in the graph. + matrix for a specific vertex in a graph. Returns: - numpyndarray: 2-dimensional and upper-triangular matrix representing - the covariance matrix between a given vertex and all other vertices - in the graph. + Tuple[bool,npndarray]: A result of computing covariance matrix and the + actual computed matrix. """ cov_vertices = [(vertex.hessian_index(), vertex.hessian_index())] @@ -658,12 +659,11 @@ def get_covariance_matrix(self, vertex): def visualize_g2o_realtime(self, optimizer): """ - Visualizes the real-time optimization process of a G2O algorithm by plotting - the positions and edges of the vertices in 3D space. + Visualizes a real-time 3D graphical representation of a G2O optimization + problem using Python's Matplotlib library. Args: - optimizer (object | Optimizer): Used to load G2O files, estimate vertex - positions, and visualize them in real-time. + optimizer (object): Used to load G2O files for visualization. """ plt.ion() @@ -712,13 +712,14 @@ def update_node_att(self, id: int, attribute_names: [str]): # print("INIT GRAPH") """ - Updates the attributes of a node in a graph, specifically the odometry queue. + Updates attributes of a node in a graph based on the current time and robot + state, and appends the updated values to an odometry queue for later use. Args: - id (int): Represented as an integer value that identifies the node to - be updated. - attribute_names ([str]): An array containing the names of attributes - to be updated for the given node ID. + id (int): Used to identify the node for which the attributes are being + updated. + attribute_names ([str]): An array of strings representing the names + of attributes to be updated on the given node ID. """ if id == self.odometry_node_id: @@ -739,25 +740,24 @@ def update_node(self, id: int, type: str): # self.room_initialized = True if self.initialize_g2o_graph() else False # self.init_graph = True """ - Updates a node with ID `id`. The method checks if the node type is "corner", - and if so, it retrieves the room node from the graph and sets a flag - indicating that the room has been initialized. + Updates the node with the given ID based on the type parameter. If the + type is "corner", it initializes a room graph and sets the `init_graph` + attribute to `True`. Args: - id (int): Used to identify the node to be updated. - type (str): Used to specify the node's type. + id (int): An identifier for the node to be updated. + type (str): Used to specify the node's type, which can be either + "corner" or any other valid value. """ pass def delete_node(self, id: int): """ - Deletes a node from a list or dict based on its ID, setting `self.room_initialized` - to `False`. + Deletes a node from a list of nodes maintained by the worker. Args: - id (int): Intended to represent the unique identifier for the node - being deleted. + id (int): Used to represent the unique identifier of the node to be deleted. """ pass @@ -770,15 +770,15 @@ def delete_node(self, id: int): def update_edge(self, fr: int, to: int, type: str): """ - Updates the room ID and sets the current edge set based on the type of - edge received from the graph. + Updates the room ID and RT translation and rotation values based on the + incoming edge type and node attributes. Args: - fr (int): Representing the starting node index of an edge in a graph. - to (int): The target node index of the edge to be updated, which can - be either a room or the Shadow node. - type (str): Used to specify the edge type, which can be either "current" - or "RT". + fr (int): The index of the current edge being processed. + to (int): Representing the index of the next node in the graph that + is being processed. + type (str): Used to specify the edge's type, which can be either + "current" or "RT". """ if type == "current" and self.g.get_node(fr).type == "room": @@ -810,17 +810,13 @@ def update_edge_att(self, fr: int, to: int, type: str, attribute_names: [str]): def delete_edge(self, fr: int, to: int, type: str): """ - Deletes an edge from a graph, specified by its index (fr) and the index - of its adjacent vertex (to). The edge's type can be specified for further - filtering. + Deletes an edge from a graph, identified by its index (fr) and type (to). Args: - fr (int): 1st argument or input of the function indicating the first - vertex or node to delete an edge from. - to (int): Used to specify the destination vertex ID for the edge - deletion operation. - type (str): Used to indicate the edge type to be deleted, with possible - values 'in' or 'out'. + fr (int): 0-based index of the edge to be deleted. + to (int): Used to specify the target vertex for edge deletion. + type (str): Used to specify the type of edge to delete, with possible + values 'direct' or 'indirect'. """ pass 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 a6b4663f..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 @@ -14,33 +14,39 @@ class LongTermGraph: """ - Acts as a plotter for a graph representation of a metric reconstruction. It - provides methods to draw rooms and doors, and to customize the plot's appearance. + 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, defaulting - to 'r-' for non-current rooms and 'g-' for current rooms. - read_graph (Callable[[Union[str,Path],int],Graph]): Used to read a graph - from a file specified by the file path or name. - fig (matplotlibfigureFigure): Used to represent the figure instance for - visualization of the graph. - ax (matplotlibpyplotAxes): Used to represent the axis of the graph. It - provides methods for setting various properties of the axes, such as - titles, labels, limits, and annotations. - fig_2 (matplotlibfigureFigure): Used to store the second figure object - that is created for visualizing the metric reconstruction. - ax_2 (matplotlibpyplotAxes): Used to access and manipulate the second axes - object created by the `subplot()` function in the parent plot. + 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 and creates an instance of the Graph class, - optionally creating a figure and axes to visualize the graph. + 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 path to a graph file from which - the graph will be read. + file_name (str): Used to specify the name of the graph file that + contains the LTSM data to be reconstructed. """ try: @@ -140,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") - print("Corners transformed", corners_transf) 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] - print("Corners in room", corners_in_room) 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)] @@ -282,12 +286,12 @@ def check_point_in_map(self, rooms_map: dict, point: QPoint): def draw_graph(self, only_rooms=True): """ - Creates a graph based on a directed graph represented by a NetworkX graph - object, and displays it using Matplotlib's scatter and plot functions. + 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 the nodes and edges of the graph - according to their types, only including rooms and doors. + 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() diff --git a/agents/long_term_spatial_memory_agent/src/specificworker.py b/agents/long_term_spatial_memory_agent/src/specificworker.py index a86380c0..132e747b 100644 --- a/agents/long_term_spatial_memory_agent/src/specificworker.py +++ b/agents/long_term_spatial_memory_agent/src/specificworker.py @@ -48,81 +48,92 @@ class SpecificWorker(GenericWorker): """ - Manages a graph representing a manipulation task, providing methods for - inserting, updating, and deleting nodes and edges. It also keeps track of the - robot's current state and performs actions based on that state. + Manages a specific node and edge attributes and performs various operations + on them, such as updating, inserting, deleting, and attributing. It also keeps + track of the current state of the robot and updates the graph accordingly. Attributes: - Period (str|int): Used to store the period of time during which the worker - performs its task, either in seconds or minutes, depending on the value - assigned to it. - agent_id (int|str): Used to store the ID of the specific worker for which - the methods of this class are intended to work. - g (igraphGraph): Used for handling long-term dependencies between nodes - in the graph. - startup_check (bool|str): Used to check if the worker has started successfully - or not. It can be set to a specific value like "success" or "failure" - to indicate whether the startup was successful or not, or it can be - left blank to default to a boolean value indicating whether the worker - started successfully or not. - rt_api (str|int): Used to store the RT (Real-Time) API of the worker, which - allows the robot to perform real-time actions. - inner_api (Dict[str,Any]): Used to store internal APIs for the worker's - methods that are not exposed to the outside world. - robot_name (str|List[str]): Used to store the name of the robot that the - worker belongs to. - robot_id (int): Used to represent the unique identifier of the robot node - in the graph. - last_robot_pose (Dict[str,float]): Used to store the last known pose of - the robot before it entered a room or crossed a boundary. - robot_exit_pose (str|int): Used to store the ID of the room node that the - robot exits through when transitioning from a door node to a room node. - state (str|bool): Used to keep track of the worker's current state (either - "idle", "crossed", or "completed"). - affordance_node_active_id (int|bool): Used to store the ID of the affordance - node that is currently active or not. - exit_door_id (int|str): Used to store the ID of the door node that serves - as an exit from a room. - room_exit_door_id (int|str): 16 by default, representing the id of a - specific door node in the long-term graph that marks the exit of a room. - enter_room_node_id (int|str): Represented as a room number that, when - updated to a value greater than zero, indicates that the robot has - entered a new room. - vertex_size (int|str): Used to store the size or dimension of a vertex in - the graph, which can be used to determine the size of a node in the graph. + Period (Union[int,str]): Used to store the period of time during which the + worker can perform its task, such as a day or a week. + agent_id (int|str): Used to identify the specific worker instance, allowing + the agent to perform tasks and maintain a unique identity during its + lifetime. + g (igraphGraph): Used to represent the long-term graph of the environment, + which is updated periodically by the worker. + startup_check (bool|str): Used to check if the worker has been started or + not, with possible values "yes", "no", or False. + rt_api (str|int): Used for storing the RT (Real-Time) API ID which represents + the current task being executed by the worker. + inner_api (Dict[str,Any]): Used to store the inner API of the worker, which + is a Python function that performs actions related to the worker's task. + robot_name (str|int): Used to store the name of the robot that will perform + a specific task. + robot_id (int): Used to identify the robot node in the graph, which is the + starting point for the worker's movement. + last_robot_pose (Tuple[float,float,float]): Stored as the last known pose + (position, orientation, and scale) of the robot before any actions + were taken in the environment. + robot_exit_pose (str|int): A reference to the pose of the robot when it + exits a room. It is used to determine when the robot has reached its + goal in a task. + state (str|int): Used to keep track of the current state of the worker, + which can be either "idle", "crossed", or "completed". + affordance_node_active_id (int|bool): Used to keep track of the active + affordance node ID, which is used to determine when the robot has + reached its goal. + exit_door_id (int|str): Used to store the ID of the door node that leads + from a room to the outside world. + room_exit_door_id (int|str): Used to represent the ID of the door node + that leads from a room to the outside world. + enter_room_node_id (int): Used to store the ID of the room node that the + worker is currently entering. + vertex_size (int|str): Used to keep track of the number of vertices in the + graph, which can be useful for various operations such as insertion, + deletion, and querying the graph. not_required_attrs (List[str]): Used to store a list of attributes that - are not required for the worker's operations. It helps to optimize the - worker's performance by ignoring these unused attributes during updates, - inserts, or deletes. + are not required for the worker's functionality, but can be useful for + debugging or other purposes. + last_save_time (datetime|str): Used to store the time when the worker's + long-term graph was last saved. It helps track when the graph was last + updated for subsequent actions. long_term_graph (igraphGraph): Used to store the long-term graph representation - of the environment, which is updated during the worker's execution. - insert_current_edge (NoneOrEdge|List[str]): Used to insert a new edge into - the graph with the specified fr and to nodes, and sets it as the current - edge if no current edge exists. - timer (int|float): Used to track the time taken by the worker to process - a task. - compute (Callable[[str,str],Any]): Used to specify a function that computes - the next action for the robot based on its current state and the environment. - update_node (Dict[str,Any]): Used to update a node in the graph based on - its ID, type (door or room), and name. It performs various actions - such as checking if a door node exists, inserting a door node, updating - the RT translation and rotation, and drawing the graph. - update_edge (Union[str,int]): Used to update the edge attributes of a - specific robot node in the graph when the node's door is opened. + of the environment, which is updated over time as new data becomes available. + initialize_room_from_igraph (Dict[str,Any]): Used to initialize the room + graph from an igraph graph. It takes the igraph graph as input and + creates a new room graph with the same node IDs and edges as the input + igraph. + update_robot_pose_in_igraph (void): Used to update the pose of a robot + node in the graph by inserting a new edge with the correct "RT" + translation and rotation. + insert_current_edge (List[Edge]): Used to insert a new edge into the graph + with the specified source and target nodes and edge type, effectively + setting it as the current edge in the graph. + timer (int|float): Used to track the time taken by the worker to perform + its tasks. + compute (Callable[[int],str|int]): Defined as a method that takes an integer + argument and returns a string or integer value representing the result + of some computation. It is used in various methods of the worker to + perform computations on the graph. + update_node (Dict[str,Any]): Responsible for updating a node in the graph + when the worker's id matches the node's id. It takes three parameters: + `id`, `type`, and `door_node`. The method updates the node's attributes + based on its type and inserts an RT edge between the node and its + parent if necessary. + update_edge (Union[int,str]): Used to update an edge's type or remove it + altogether based on certain conditions. """ def __init__(self, proxy_map, startup_check=False): """ - Initializes instance variables and sets up event listeners for graph - updates, timeouts, and node measurements. + Initializes the worker's internal state, including its graph, node and + edge variables, and timers for computing and updating the graph. Args: - proxy_map (Dict[str, Any]): Used to pass additional data to the - SpecificWorker object. It contains key-value pairs that can be - used to store any information required by the worker. - startup_check (bool): Used to check if the agent has already started - its computation, skipping the computation if it has already been - done. + proxy_map (dict): Used to pass a mapping of proxy nodes to their + corresponding real nodes in the graph. + startup_check (bool): Set to False by default. It performs a check on + the graph upon initialization if it's set to True, otherwise, it + does not. """ super(SpecificWorker, self).__init__(proxy_map) @@ -160,6 +171,7 @@ def __init__(self, proxy_map, startup_check=False): 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") # Check if self.long_term_graph.g is empty @@ -169,6 +181,8 @@ def __init__(self, proxy_map, startup_check=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() @@ -200,13 +214,11 @@ def __del__(self): def setParams(self, params): """ - Sets the parameters passed as an argument to `True`. Then, it removes a - self-edge from the current room, stores the ID of the exit door in a - variable, and sets the name attribute of both doors to the other side - door's name. + Sets parameters and modifies the state of the room by removing a self-edge, + storing an exit door ID, and adding attributes to doors. Args: - params (bool): Passed to set parameters for an object of class `Room`. + params (bool): Passed to set the parameters of the room. Returns: bool: True. @@ -227,10 +239,79 @@ 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): + # Exectue function self.update_robot_pose_in_igraph each second + """ + Updates the robot's pose in the graph and performs various actions based + on the current state of the robot. + + """ + if time.time() - self.last_save_time > 1: + self.update_robot_pose_in_igraph() + + 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 initialize_room_from_igraph(self): + # Remove RT edge between "root" and "Shadow" + """ + Initializes a room in a graph by deleting an edge, finding the robot node + and its neighbors, creating a new vertex for the root of the igraph, + inserting edges to connect the robot to the room and the room to itself, + and updating the robot's parent node attribute. + + """ + 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 """ - Computes and updates the robot's RT (reactive transport) based on the - current state of the graph. + Updates the robot's pose in an igraph graph based on a RT translation and + rotation, taking into account the robot's current location and the surrounding + rooms. """ if self.long_term_graph.g: @@ -272,8 +353,6 @@ def compute(self): except Exception as e: print(e) - print("1") - except Exception as e: print(e) print("No robot node found in igraph. Inserting") @@ -281,34 +360,17 @@ def compute(self): 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() - + # 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 """ - Computes the affordance pose of the robot in a new room based on its current - position and orientation, and associates doors in the long-term graph with - their corresponding rooms. + Detects when the robot enters a new room and associates doors based on + their proximity to the robot's current position, updating the graph and + door attributes accordingly. """ aff_cross_nodes = [node for node in self.g.get_nodes_by_type("affordance") if node.attrs["active"].value == True] @@ -411,8 +473,9 @@ def idle(self): def crossed(self): # Get parent node of affordance node """ - Updates the state of the worker based on the active affordance node and - exit door ID. + 1) retrieves the active affordance node and its parent, 2) sets the exit + door ID to the value of the parent attribute, and 3) deletes the edge + connecting the current room to itself. """ affordance_node = self.g.get_node(self.affordance_node_active_id) @@ -437,8 +500,9 @@ def initializing_room(self): # Get room nodes """ - Initializes the room nodes and sets the enter room node ID, then transitions - to the "initializing doors" state. + Determines and sets the ID of the room node to enter upon construction, + based on conditions involving the existence of nodes labeled "room" and + the absence of certain attributes. """ 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] @@ -455,9 +519,9 @@ def initializing_room(self): def known_room(self): # Get other side door name attribute """ - Determines the room name associated with a door based on its ID and updates - the graph with new edges and nodes to simulate robot movement through the - door and into the adjacent room. + Detects the room the robot is currently in and finds the door node leading + to the next room. It then creates a new edge in the graph representing the + RT pose of the robot and updates the graph with the new edge. """ other_side_door_node = self.g.get_node(self.exit_door_id) @@ -555,9 +619,9 @@ def known_room(self): def initializing_doors(self): # Check if node called "room_entry" of type room exists """ - 1) retrieves exit edges and matching doors, 2) sets other side door name - and connected room name attributes for each exit door node, and 3) associates - doors using their names and connected rooms. + 1) retrieves edges leading to the exit door, 2) checks if any matching + edges exist, and 3) updates node attributes and associates doors based on + the matched edges. """ exit_edges = [edge for edge in self.g.get_edges_by_type("exit") if edge.destination == self.exit_door_id] @@ -600,14 +664,14 @@ def initializing_doors(self): 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 their nodes and - updates their attributes to reflect the connection. + Connects two nodes representing doors in the long-term graph, adding an + edge between them and updating their attributes to reflect the connection. Args: door_1 (str): A list containing the name of the first door to be - associated with the long-term graph. - door_2 (str): A name of another door in the graph, which will be - connected to the current door node through an edge in the graph. + associated with another door. + door_2 (str): A string representation of the second door to be associated + with the long-term graph. """ try: @@ -629,8 +693,7 @@ def associate_doors(self, door_1, door_2): def store_graph(self): """ - Saves the graph representation of a room to a file named "graph.pkl" using - Python's pickle module. + Saves the graph representation of a room node and its exit door id to a file. """ actual_room_node = self.g.get_node(self.room_exit_door_id) @@ -649,8 +712,8 @@ def store_graph(self): def removing(self): # # Get last number in the name of the room """ - Removes edges from the long-term graph based on room numbers and updates - the state of the worker. + Removes edges from the long-term graph based on certain conditions, including + room numbers and types of edges. """ room_number = self.g.get_node(self.room_exit_door_id).attrs["room_id"].value @@ -680,13 +743,14 @@ def removing(self): def traverse_graph(self, node_id): # Mark the current node as visited and print it """ - Traverses a graph by starting at a designated node and following RT edges - to reach other nodes, inserting vertices and edges into an IGraph object - as it goes. + Traverses a graph and performs actions based on the type of edges it + encounters. It starts at a designated node, then visits the children nodes + of that node according to the RT edge type, and repeats this process for + each child node until no more actions are left to perform. Args: - node_id (int): Used to represent the unique identifier for a node in - the graph. + node_id (int): Represented by the igraph vertex ID of the current node + being traversed. """ node = self.g.get_node(node_id) @@ -699,13 +763,11 @@ def traverse_graph(self, node_id): def traverse_igraph(self, node): """ - Traverses the graph by visiting all vertices that have successors in the - long-term graph, and for each such vertex, it checks if its successor has - a higher level than the current vertex, and if so, it inserts a DSR vertex - and edge between them, and recursively traverses the successor. + Traverses the graph, identifying vertices with higher level than the current + vertex and adding them to the DSR graph along with their connections. Args: - node (igraph.Vertex): Used to represent the current node being traversed. + node (igraph.Vertex): Representing a vertex in the graph. """ vertex_successors = self.long_term_graph.g.successors(node.index) @@ -722,11 +784,12 @@ def traverse_igraph(self, node): def insert_igraph_vertex(self, node): """ - Adds a new vertex to an existing graph, updating its attributes and edges - based on node-specific data. + Adds a new vertex to an existing graph, updating the graph's vertices with + provided attributes and matching other vertices based on specified conditions. Args: - node (igraph.Node): Used to insert a new vertex into the graph. + node (igraph.Vertex): Used to represent a single vertex in the graph, + including its name, ID, and attributes. """ self.long_term_graph.g.add_vertex(name=node.name, id=node.id, type=node.type) @@ -766,15 +829,14 @@ def insert_igraph_vertex(self, node): def insert_dsr_vertex(self, parent_name, node): # print("Inserting vertex", node["name"], node["type"]) """ - Inserts a new node into a graph, updating the parent node's attribute with - the agent ID and copying over non-required attributes from the input node - to the new node. + Modifies an existing node within a graph, adding new attributes and linking + it to its parent node through an attribute. Args: - parent_name (str): Used to specify the name of the parent node to which - the new node will be added. - node (Node | dict): Passed the attributes and data of a vertex to be - inserted into a graph. + parent_name (str): Used to specify the name of the parent node to + insert the new vertex into. + node (Node | dict): Passed in from caller, it contains attributes that + define the vertex to be inserted into graph. """ new_node = Node(agent_id=self.agent_id, type=node["type"], name=node["name"]) @@ -791,12 +853,15 @@ def insert_dsr_vertex(self, parent_name, node): id_result = self.g.insert_node(new_node) def insert_igraph_edge(self, edge): """ - Adds an edge to a long-term graph based on a given edge attribute, updating - the node positions and rotation accordingly. + Adds an edge to a long-term graph based on a received edge attribute. It + checks if the destination node is the robot, and if so, uses the correct + translation and rotation attributes. Otherwise, it uses the provided + translation and rotation attributes. Args: - edge (rt.Edge): Passed by reference to the method, representing an - edge with attributes such as translation and rotation. + edge (GraphEdge): Passed as a whole object containing information about + an edge in the graph, including its origin and destination nodes, + as well as attribute values such as translation and rotation. """ origin_node_dsr = self.g.get_node(edge.origin) @@ -819,15 +884,16 @@ def insert_dsr_edge(self, org, dest): # print("ORG::", org) # self.insert_dsr_vertex(dest) """ - Modifies an existing graph by adding an edge with specific RT attributes, - based on input org and dest nodes. + Creates a new edge in the long-term graph based on data from the short-term + graph. It assigns attributes to the new edge, including a translation and + rotation value for RT tracking. Args: - org (GraphNode | NoneType): Used to represent the origin node of the - edge to be inserted. If it is None, the function will use the root - node as the origin. - dest (Node | str): Used to specify the destination node or name in the - graph for insertion of an edge. + org (GraphNode | NoneType): Used to specify the starting node of the + edge to be inserted. If it is None, it means the starting node is + unknown or unspecified. + dest (Node | str): Used to specify the destination node or edge id for + the edge insertion operation. """ if org is None: @@ -859,14 +925,17 @@ def insert_dsr_edge(self, org, dest): def check_element_room_number(self, node_id): """ - Determines the room number of an element by retrieving the attribute - 'room_id' from the element's node ID. + Determines the room number associated with a given node ID by retrieving + the attribute "room_id" from the node and returning its value if found, + or -1 otherwise. Args: - node_id (str): Used to reference a node in the graph. + node_id (int): Passed as an argument to the function, indicating the + id of the element whose room number needs to be checked. Returns: - int: The room ID associated with a given node ID. + int: The room ID associated with a given node ID if the node has an + attribute "room_id" and its value is not None, or -1 otherwise. """ node = self.g.get_node(node_id) @@ -879,15 +948,16 @@ def check_element_room_number(self, node_id): def check_element_level(self, node_id): """ - Determines the element level of a node based on its attribute "level" and - returns it if found, or -1 otherwise. + 1) retrieves the element level of a node with the given ID, and 2) handles + missing or non-existent element levels by printing an error message and + returning -1. Args: - node_id (int): Represented by the identifier `node_id`. It serves as - an index into the graph's node list to access the specified node. + node_id (int): Representing an element node identifier, which identifies + a specific element in the graph. Returns: - int: Element level of the node with given id + int: Element level if found, or -1 otherwise. """ node = self.g.get_node(node_id) @@ -935,13 +1005,13 @@ def check_element_level(self, node_id): def generate_room_picture(self, room_node_id): """ - 1) retrieves room information from the graph, 2) identifies RT edges - connecting to the current room, and 3) extracts translation attribute for - the current room. + Retrieves and processes information about a room node in a graph, including + its ID, old RT edges, translation attribute, and doors. It then draws the + room polygon and doors on an image for saving. Args: - room_node_id (str | int): Required, indicating that it is mandatory - to pass this value for the function to work correctly. + room_node_id (str | int): Used to identify the room for which the + picture will be generated. """ room_node = self.g.get_node(room_node_id) @@ -980,13 +1050,13 @@ def generate_room_picture(self, room_node_id): def insert_current_edge(self, room_id): # Insert current edge to the room """ - Inserts or assigns an edge in the graph representing the room hierarchy, - with the current worker's ID as the source and the current room ID as the - target. + Inserts or assigns an edge in the graph with the current room ID as the + tail and the same ID as the head, indicating that the agent is currently + located in that room. Args: - room_id (str): Used as an identifier for the current room that the - agent is located in. + room_id (str): Used to represent the id of the current room in which + the agent is located. """ current_edge = Edge(room_id, room_id, "current", self.agent_id) @@ -1003,16 +1073,13 @@ def update_node_att(self, id: int, attribute_names: [str]): def update_node(self, id: int, type: str): """ - Updates a node in the graph based on its ID and type. If the type is "door", - it inserts a vertex in the long-term graph, checks if a room node exists, - and inserts an edge with a specific translation and rotation. If the ID - is the affordance node active ID, it checks the state and activeness of - the node and transitions to the crossed state. + Updates the node attributes based on user input and inserts new edges into + the graph according to predefined rules. Args: - id (int): Used to identify the node being updated. - type (str): Used to identify the node being updated, with possible - values of "door" or "room". + id (int): Used to identify the node to be updated. + type (str): Used to identify the type of node being updated (door or + room). """ if type == "door": @@ -1074,16 +1141,14 @@ def update_edge(self, fr: int, to: int, type: str): # TODO: Posible problema si tocas el nodo room en la interfaz gráfica """ - Updates the current edge based on the specified frame, to node, and type. - If no current edge exists, it inserts a new edge and sets it as the current - one. + Updates an edge in the graph based on its type, source node, and target node. Args: - fr (int): Representative of the 'from' node value, which indicates the - starting point of an edge - to (int): The target node ID of an edge to be updated. - type (str): Used to specify the type of edge being updated, either - "RT" for real-time or "NRT" for non-real-time. + fr (int): Representing the first room number of an edge to be updated + with new information. + to (int): Used to identify the target node in the graph for updating + the edge. + type (str): Used to indicate whether the edge is a robot or a room node. """ 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: From ce076578eb3335b46f3699899b1a347e61c2791c Mon Sep 17 00:00:00 2001 From: "komment-ai[bot]" <122626893+komment-ai[bot]@users.noreply.github.com> Date: Fri, 19 Jul 2024 07:43:14 +0000 Subject: [PATCH 09/10] Added comments to 12 functions across 1 file --- .komment/00000.json | 259 ++++++++++---------- .komment/komment.json | 5 +- agents/g2o_agent/src/specificworker.py | 326 ++++++++++++------------- 3 files changed, 293 insertions(+), 297 deletions(-) diff --git a/.komment/00000.json b/.komment/00000.json index 1141c341..4d292665 100644 --- a/.komment/00000.json +++ b/.komment/00000.json @@ -251,192 +251,187 @@ "path": "agents/g2o_agent/src/specificworker.py", "content": { "structured": { - "description": "a class called `RtOdometry` that implements an algorithm for estimating the position and orientation of a robot based on its odometry data. The code uses the `gazebo` package to simulate the robot's environment and perform odometry calculations. It also utilizes the `rosbag` package to read and process ROS bag files containing sensor data.\n\nThe algorithm first initializes the robot's position and orientation based on the starting pose of the simulation. Then, it updates the node and edge information in the graph using the odometry data from the ROS bag file. Specifically, it calculates the current side speed and angular speed of the robot, and updates the node's position and orientation based on these values. It also updates the edges in the graph based on the current edge set and the RT transmission information.\n\nThe code then implements a few methods for updating and deleting nodes and edges in the graph, as well as updating the algorithm's state based on new sensor data. Overall, the code provides a basic implementation of an odometry algorithm that can be used to estimate the position and orientation of a robot in a simulated environment.", + "description": "a class called `Robot` that interacts with a graph representing a robot's environment and its movements. The class has several methods for adding, updating, and deleting nodes and edges in the graph, as well as updating edge attributes. It also keeps track of the robot's current position and orientation, and uses this information to update its graph representation.\n\nThe code uses the `networkx` library to represent the graph, and the `time` module for timing purposes. The class has methods to initialize and update the graph, as well as to handle various events such as new nodes or edges being added, or a room changing. It also has methods to delete nodes and edges when they are no longer needed.\n\nOverall, the code is concerned with maintaining an accurate representation of the robot's environment and movements, and updating this representation in response to changes in the graph.", "items": [ { - "id": "cdcb03d8-8236-2faa-f349-0e323183c24c", + "id": "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359", "ancestors": [], - "description": "Manages a specific worker's tasks and updates in a graph. It has methods for updating node attributes, deleting nodes, and setting up RT transmission, as well as checking for updates and handling errors.", + "description": "Manages a graph-based representation of an environment and its objects, providing functions for updating node and edge attributes, deleting nodes and edges, and handling RT messages. It also maintains a set of rooms and their initial positions.", "attributes": [ { "name": "Period", "type_name": "float|int", - "description": "Used to control the update rate of the graph. It determines how often the graph is updated, with a higher value resulting in more frequent updates." + "description": "Set to `200` by default, indicating the time interval (in milliseconds) between successive calls to the `update` method. It determines how frequently the worker updates its state." }, { "name": "agent_id", "type_name": "int|str", - "description": "Used to store the ID of the agent that will be using the specific worker." + "description": "Used to identify the worker's agent in the simulation environment." }, { "name": "g", - "type_name": "Graph|QGraphicsScene", - "description": "Used to store the graph data for the worker's algorithm, including the nodes, edges, and their attributes." + "type_name": "Graph|NetworkXGraph", + "description": "Used for managing graphs in the worker's update and delete operations." }, { "name": "startup_check", "type_name": "QTimersingleShot200,QApplicationinstancequit", - "description": "Used to check if the application is still running after a certain time period (200 milliseconds) and quit if so." + "description": "Used to check if the application should quit after a certain period of time." }, { "name": "rt_api", - "type_name": "Union[str,int]", - "description": "Used to store the RT (Real-time) edge set ID for the worker's specific room." + "type_name": "str|int", + "description": "Used to store the last RT (Real-time) edge set id that was received by the worker, so it can check if a new RT edge set has been received since the last time it was checked." }, { "name": "inner_api", "type_name": "Callable[[],None]", - "description": "Used to define a custom API for the worker to interact with its internal data structures and algorithms without exposing them to the outer world." + "description": "Used to define a custom API for the worker to interact with its internal state and update its graph." }, { "name": "odometry_node_id", "type_name": "int", - "description": "Used to identify the node in the graph that represents the robot's odometry information." + "description": "Used to store the ID of the node representing the shadow robot in the graph, which is used for updating the robot's odometry." }, { "name": "odometry_queue", "type_name": "List[Tuple[float,float,float,int]]", - "description": "Used to store odometry data from the environment, containing the robot's current advance speed, side speed, and angular speed, as well as the timestamp in milliseconds since the epoch." + "description": "Used to store the odometry data points received from the ROS node. It is updated every 200 milliseconds." }, { "name": "last_odometry", "type_name": "float|List[float]", - "description": "Used to store the last known odometry values (position, orientation, and translation) of the robot, which are updated in real-time during the simulation." + "description": "Used to store the last known odometry values (position, orientation, and advance speed) for the robot." }, { "name": "g2o", - "type_name": "Graph|Tuple[str,str]", - "description": "Used to represent the graph structure of the robot's environment. It contains a tuple of two elements: the first element is the name of the graph file, and the second element is the name of the node file." - }, - { - "name": "visualizer", - "type_name": "Callable[[QWidget],None]", - "description": "Used to create a QWidget instance for visualizing the robot's movement and orientation in real-time." + "type_name": "None|Graph2O", + "description": "Used to store the graph data in Graph2O format for the robot's environment." }, { "name": "odometry_noise_std_dev", "type_name": "float|int", - "description": "0.1 by default, representing the standard deviation of noise added to the odometry measurements for the robot's position, orientation, and angular velocity." + "description": "Used to control the noise level in robot odometry readings." }, { "name": "odometry_noise_angle_std_dev", - "type_name": "float|double", - "description": "0.2 by default, representing the standard deviation of angle noise in the odometry estimates. It affects how much the worker's estimate of the agent's position and orientation deviates from the true values due to random noise in the sensor readings." + "type_name": "float|int", + "description": "0.1 by default, representing the standard deviation of the angle noise added to the robot's odometry measurements during simulation." }, { "name": "measurement_noise_std_dev", - "type_name": "float|int", - "description": "0.1 by default, indicating the standard deviation of the noise present in the robot's measurements." + "type_name": "float|double", + "description": "Used to represent the standard deviation of measurement noise in the worker's measurements. It is used to simulate random fluctuations in the measurements during training." }, { "name": "last_room_id", "type_name": "int|str", - "description": "Used to store the room ID of the previous room that the agent has entered before changing rooms, which is useful for determining when the agent has moved to a new room." + "description": "Used to store the last room ID seen by the worker before changing rooms, which allows the worker to track the current room ID." }, { "name": "actual_room_id", - "type_name": "int|str", - "description": "Used to store the current room ID of the agent, which is updated when the agent moves to a new room." + "type_name": "str|int", + "description": "Used to keep track of the current room ID that the worker is in, during its execution" }, { "name": "elapsed", "type_name": "float|int", - "description": "Used to store the time since the last call to the `update()` method, which is used to update the node's attributes." + "description": "Used to store the elapsed time since the worker's last call to the `update` method, which can be used to control the worker's execution rate." }, { "name": "room_initialized", "type_name": "bool", - "description": "Set to False when a room change is detected, indicating that the worker has not yet initialized its current room's graph. It is reset to True once the worker has successfully initialized the new room's graph." + "description": "Used to track whether a specific room has been initialized for RT mapping. It is set to False when the room is first encountered, and True when it has been successfully mapped with the robot's pose." }, { "name": "iterations", "type_name": "int|float", - "description": "Used to keep track of the number of iterations (i.e., frames) that the worker has processed. It is updated with each frame processed by the worker, and can be used to control the speed of the worker or to implement various algorithms." + "description": "Used to keep track of the number of iterations of the worker's tasks that have been performed." }, { "name": "hide", "type_name": "bool", - "description": "Used to indicate whether the worker should be hidden or not, affecting its visibility in the graph." + "description": "Used to hide or show the worker's updates during simulation." }, { "name": "init_graph", "type_name": "bool", - "description": "Used to indicate whether the graph has been initialized or not. It is set to True when the worker is initialized and False otherwise, allowing for proper handling of the graph and its nodes and edges in the update methods." + "description": "Set to True when the worker initializes its graph and False otherwise, indicating whether the graph has been initialized or not." }, { "name": "current_edge_set", "type_name": "bool", - "description": "Used to track whether an edge set has been computed for the current room id. It is set to True when the first RT edge set is computed and False otherwise, allowing the worker to avoid recomputing the edge set if it has already been computed for the same room id." + "description": "Used to keep track of whether the current edge set has been updated recently, indicating when a new RT translation or rotation should be computed. It is set to True after each edge set update and reset to False after a certain time interval (defined as `rt_time_min` in the code) has passed without any updates, to avoid computing unnecessary RT translations or rotations." }, { "name": "first_rt_set", "type_name": "bool", - "description": "Set to `True` when the RT translation and rotation values are being set for the first time, and `False` otherwise." + "description": "Set to True when the RT translation and rotation are first detected, False otherwise. It indicates whether the RT set has been detected for a given edge." }, { "name": "translation_to_set", "type_name": "float|List[float]", - "description": "Used to store the translation values set by the RT algorithm." + "description": "Used to store the translation of the robot to set. It is updated when the RT transmission is received and its value represents the distance from the origin of the robot's frame to the origin of the set frame in the global coordinate system." }, { "name": "rotation_to_set", - "type_name": "float|int", - "description": "A value representing the rotation of the robot to set its end effector at a specific location." + "type_name": "float|List[float]", + "description": "Used to store the Euler angles representing the rotation from the current room to the target set." }, { "name": "room_polygon", "type_name": "List[float]", - "description": "Used to store the vertices of a polygon that represents the room boundary in the environment." + "description": "Used to store the coordinates of a polygon representing the boundary of a room in the environment, which is used for collision detection and avoidance." }, { "name": "security_polygon", - "type_name": "Union[int,List[float]]", - "description": "Used to store the security polygon of the worker's workspace in 3D space. It is used to detect collisions between the worker's manipulator and objects in the environment." + "type_name": "Shape|str", + "description": "Used to store a polygon that defines the security area around the robot, which is used for collision detection and obstacle avoidance." }, { "name": "initialize_g2o_graph", "type_name": "void", - "description": "Used to create a new graph for the given worker instance, which will be used to store the robot's position and orientation in the environment." + "description": "Used to initialize a Graph2O graph, which is a data structure representing a robot's environment, and perform other operations such as adding nodes and edges, setting attributes, and updating node positions." }, { "name": "rt_set_last_time", - "type_name": "int|float", - "description": "Used to store the last time a RT (Real-Time) edge was set for a specific agent ID. It is used in conjunction with the `rt_time_min` attribute to determine when to reset the RT translation and rotation values." + "type_name": "float|int", + "description": "Used to track the time since the last RT set was created by the worker. It is used to determine when to set a new RT translation and rotation value." }, { "name": "rt_time_min", - "type_name": "float", - "description": "Defined as the minimum time duration between two consecutive RT sets in milliseconds. It is used to determine when to reset the translation and rotation values for the shadow agent during RT tracking." + "type_name": "float|int", + "description": "Defined as the minimum time interval between consecutive RT sets. Its purpose is to ensure that the robot's motion is smooth and does not oscillate excessively during navigation." }, { "name": "last_update_with_corners", "type_name": "int|bool", - "description": "Used to keep track of the last time a node or edge update was performed with corner information. It is initially set to False, and its value changes when updates are performed with corners. Its purpose is to check if the worker has updated with corners before, so as to avoid unnecessary updates in the future." + "description": "Used to keep track of the last time corners were updated. When the corner of a room was updated, it's set to True, otherwise it's set to False." }, { "name": "timer", - "type_name": "float|int", - "description": "Used to schedule a call to the `QApplication.instance().quit()` function after 2 seconds." + "type_name": "QTimersingleShot200,QApplicationinstancequit", + "description": "Used to schedule a call to the `QApplication.instance().quit()` function after 200 milliseconds. This allows the worker to run indefinitely until the user presses the \"Stop\" button." }, { "name": "compute", - "type_name": "Callable[[],Any]", - "description": "Used to define a function that computes the worker's task based on the graph and other attributes." + "type_name": "Callable[[],float]", + "description": "Used to compute the next node id to update based on the current node id and the edge id. It takes no arguments and returns a floating-point value representing the next node id to update." }, { "name": "update_node_att", - "type_name": "Callable[int,[str]]", - "description": "Used to update the attributes of a specific node in the graph based on its ID and the type of update. It takes two arguments: id, which is the ID of the node to be updated, and attribute_names, which is a list of attribute names to be updated." + "type_name": "List[str]", + "description": "Used to update the attributes of a node in the graph based on certain conditions, such as when the node's id matches the `odometry_node_id` constant." }, { "name": "update_edge", - "type_name": "Callable[int,str,str]", - "description": "Used to update the attributes of an edge in the graph based on its type (current or RT translation) and the node it connects." + "type_name": "Callable[[int,int,str],None]", + "description": "Called when an edge's type changes to \"RT\". It sets the `translation_to_set`, `rotation_to_set`, and `rt_set_last_time` attributes based on the new RT translation and rotation." }, { "name": "update_edge_att", - "type_name": "Tuple[str,]", - "description": "Used to update the attributes of an edge in a graph based on its type and the name of the attribute." + "type_name": "List[str]", + "description": "Used to update the attributes of an edge based on a specific condition. It takes three parameters: `fr`, `to`, and `type`, which are the ID of the edge being updated, its destination ID, and the type of update respectively." } ], "name": "SpecificWorker", @@ -448,32 +443,32 @@ "comment": null }, "item_type": "class", - "length": 425, + "length": 423, "docLength": null }, { - "id": "d8d35337-69aa-7fa4-5141-35f2de87d6c9", + "id": "1c3f1211-7524-8695-b942-f5e904197296", "ancestors": [ - "cdcb03d8-8236-2faa-f349-0e323183c24c" + "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" ], - "description": "Initializes instance variables and sets up event handling for signals related to node and edge attributes, as well as a timer to compute the worker's output every `Period` seconds.", + "description": "Initializes various components of the worker, including the graph, odometry queue, and timer. It also sets up signal connections for updates on nodes and edges.", "params": [ { "name": "proxy_map", "type_name": "Dict[str, Any]", - "description": "Used to store the mapping between the original node attributes and their proxies." + "description": "Used to store map-related information, such as the graph's nodes and edges, which are required for the worker's functionality." }, { "name": "startup_check", "type_name": "bool", - "description": "Used to check if the graph has already been initialized before starting the worker's execution." + "description": "Used to check if the graph has been properly initialized before proceeding with the worker's tasks." } ], "returns": null, "usage": { "language": "python", - "code": "# Create an instance of the SpecificWorker class\nmy_specific_worker = SpecificWorker(proxy_map)\n\n# Set up the startup check\nmy_specific_worker.startup_check()\n\n# Connect signals to update node attributes and edges\nsignals.connect(my_specific_worker.g, signals.UPDATE_NODE_ATTR, my_specific_worker.update_node_att)\nsignals.connect(my_specific_worker.g, signals.UPDATE_EDGE, my_specific_worker.update_edge)\n", - "description": "" + "code": "# Initialize the class with the proxy_map and startup check parameter set to False\nworker = SpecificWorker(proxy_map, startup_check=False)\n\n# Set the agent ID and initialize the graph\nworker.agent_id = 20\nworker.initialize_graph()\n\n# Connect the signals for updating node attributes, deleting nodes, and updating edges\nsignals.connect(worker.g, signals.UPDATE_NODE_ATTR, worker.update_node_att)\nsignals.connect(worker.g, signals.DELETE_NODE, worker.delete_node)\nsignals.connect(worker.g, signals.UPDATE_EDGE, worker.update_edge)\nsignals.connect(worker.g, signals.UPDATE_EDGE_ATTR, worker.update_edge_att)\n", + "description": "\nThis code initializes the SpecificWorker class with a proxy map and sets its agent ID to 20. It then connects the signals for updating node attributes, deleting nodes, and updating edges using the signals module." }, "name": "__init__", "location": { @@ -484,21 +479,21 @@ "comment": null }, "item_type": "constructor", - "length": 56, + "length": 54, "docLength": null }, { - "id": "8a46ef7f-7071-d3b0-3741-830cddb99e31", + "id": "ee0a9d1b-9271-b39a-5e4f-64e1d858f292", "ancestors": [ - "cdcb03d8-8236-2faa-f349-0e323183c24c" + "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" ], - "description": "Computes and updates the robot's position, orientation, and covariance matrix based on RT messages received from the environment.", + "description": "Performs the following tasks:\n\n1. Checks if enough time has passed since the last frame was computed.\n2. Updates the robot's position and orientation using odometry data.\n3. Generates a new RT edge for the robot based on its current position and orientation.\n4. Adds the new RT edge to the graph.", "params": [], "returns": null, "usage": { "language": "python", - "code": "import time\nfrom specific_worker import SpecificWorker\n\n# Create a SpecificWorker object\nspecific_worker = SpecificWorker()\n\n# Call the compute method\nspecific_worker.compute(time.time())\n", - "description": "" + "code": "worker = SpecificWorker() # Create a new instance of the worker class\nworker.compute() # Call the compute method of the worker with no arguments\n", + "description": "\nThis code creates a new instance of the `SpecificWorker` class and calls its `compute()` method, which is expected to perform some computation using the data stored in the worker's variables. However, without knowing more about the implementation details of the worker's `compute()` method, it is difficult to provide a more detailed example of how this function might be used by an end-user." }, "name": "compute", "location": { @@ -513,19 +508,19 @@ "docLength": null }, { - "id": "be89d23a-daac-3db1-8843-67ae2a27d66e", + "id": "433924a0-c643-1c86-c947-e424e514e8fb", "ancestors": [ - "cdcb03d8-8236-2faa-f349-0e323183c24c" + "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" ], - "description": "Initializes a Graph-Based Odometry (G2O) graph for a specific room by:\n\n1. Retrieving node and edge information from a robot's sensor data.\n2. Filtering out invalid nodes and edges based on their position and orientation.\n3. Adding nominal corners and fixed poses to the G2O graph using the retrieved data.", + "description": "1) retrieves nodes and edges from the ROS topic, 2) validates node and edge presence, 3) adds nominal corners and fixed poses to the G2O graph for robot and door nodes, and 4) initializes last odometry timestamp.", "params": [], "returns": { "type_name": "bool", - "description": "True if the g2o graph is successfully initialized and False otherwise." + "description": "1 if the initialization of the G2O graph was successful, and 0 otherwise." }, "usage": { "language": "python", - "code": "from genericworker import SpecificWorker\n\n# Create a graph and assign it to g2o variable\ng = DSRGraph(0, \"G2O_agent\", agent_id)\ng2o = G2OGraph(verbose=False)\n\n# Use initialize_g2o_graph function from genericworker module\nSpecificWorker.initialize_g2o_graph(g2o)\n", + "code": "# create graph and object for g2o\ng = DSRGraph(0, \"G2O_agent\", agent_id)\nspecific_worker = SpecificWorker(proxy_map, startup_check=False)\nspecific_worker.rt_api = rt_api(g)\nspecific_worker.inner_api = inner_api(g)\n\n# initialize g2o graph\nspecific_worker.initialize_g2o_graph()\n", "description": "" }, "name": "initialize_g2o_graph", @@ -541,26 +536,26 @@ "docLength": null }, { - "id": "9cb9cdad-12a5-6da4-1440-34306b05a309", + "id": "1a1a98ec-b136-4690-5d41-202f10833328", "ancestors": [ - "cdcb03d8-8236-2faa-f349-0e323183c24c" + "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" ], - "description": "Computes the displacement of a robot based on its odometry data, taking into account the advance, lateral movement, and angular velocity of the robot.", + "description": "Calculates the displacement of a robot based on its odometry data, computing the lateral displacement, forward displacement, and angular displacement.", "params": [ { "name": "odometry", "type_name": "Tuple[float, float, float]", - "description": "An immutable sequence containing the robot's odometry data at each time step, consisting of the linear displacement, lateral displacement, and angular velocity, respectively." + "description": "A sequence of 3-element tuples representing the robot's position, velocity, and timestamp." } ], "returns": { "type_name": "Tuple[float,float,float]", - "description": "The displacement along the x, y and z axes, respectively, calculated using the odometry data from a robot's sensors." + "description": "3 components of displacement (lateral, avance and angular) calculated based on the odometry data." }, "usage": { "language": "python", - "code": "# Create an instance of SpecificWorker class and set the proxy map\nspecific_worker = SpecificWorker(proxy_map)\n\n# Set the last odometry to a value\nlast_odometry = (1, 2, 3, 4)\n\n# Call the get_displacement function with the last odometry as an argument\ndisplacement = specific_worker.get_displacement(last_odometry)\n", - "description": "\nIn this example, we create an instance of the SpecificWorker class and set its proxy map. We then set the last odometry to a value and call the get_displacement function with that value as an argument. The function returns the displacement calculated from the odometry data." + "code": "specific_worker = SpecificWorker(proxy_map)\nodometry = (10, 20, 30, time.time())\ndesplzamiento_lateral, desplzamiento_avance, desplzamiento_angular = specific_worker.get_displacement(odometry)\nprint(\"Desplazamiento lateral:\", desplzamiento_lateral)\nprint(\"Desplazamiento avance:\", desplzamiento_avance)\nprint(\"Desplazamiento angular:\", desplzamiento_angular)\n", + "description": "" }, "name": "get_displacement", "location": { @@ -575,25 +570,25 @@ "docLength": null }, { - "id": "e5fedf8a-150c-fd98-d148-a6df1ea4a074", + "id": "70fffc25-7e1d-52b1-f24e-5a8cb6ce551c", "ancestors": [ - "cdcb03d8-8236-2faa-f349-0e323183c24c" + "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" ], - "description": "Computes the covariance matrix of a set of vertices in a graph, using an optimization algorithm to compute the marginals of the vertices and then constructing the covariance matrix from the resulting upper triangle.", + "description": "Computes the covariance matrix of a set of vertices in a graph using the G2O optimizer and returns the result.", "params": [ { "name": "vertex", - "type_name": "G2O.Vertex | G2O.HessianIndex", - "description": "Used to compute the covariance matrix for a specific vertex in a graph." + "type_name": "G2O.HessianIndex", + "description": "Used to represent a vertex in the graph." } ], "returns": { "type_name": "Tuple[bool,npndarray]", - "description": "A result of computing covariance matrix and the actual computed matrix." + "description": "A result of computing marginals and a covariance matrix." }, "usage": { "language": "python", - "code": "import sys\nfrom specific_worker import SpecificWorker\n\n# Create a new instance of SpecificWorker\nworker = SpecificWorker()\n\n# Set the proxy map and startup check to True\nproxy_map = \"proxy_map\"\nstartup_check = True\n\n# Initialize the worker with the given parameters\nworker.init(proxy_map, startup_check)\n\n# Get the covariance matrix of a vertex in the graph\nvertex = worker.g2o.graph.vertices[0]\ncovariance_matrix = worker.get_covariance_matrix(vertex)\nprint(\"Covariance matrix:\", covariance_matrix)\n", + "code": "import numpy as np\nfrom specific_worker import SpecificWorker\n\n# Create a new instance of SpecificWorker class.\nspecific_worker = SpecificWorker(proxy_map)\n\n# Get covariance matrix for a vertex with index 0.\ncov_vertices = [(specific_worker.vertex.hessian_index(), specific_worker.vertex.hessian_index())]\ncovariances, covariances_result = specific_worker.g2o.optimizer.compute_marginals(cov_vertices)\nif covariances_result:\n print(\"Covariance computed\")\n matrix = covariances.block(specific_worker.vertex.hessian_index(), specific_worker.vertex.hessian_index())\n upper_triangle = np.triu(matrix) \n print(\"Covariance matrix\", upper_triangle)\nelse:\n print(\"Covariance not computed\")\n", "description": "" }, "name": "get_covariance_matrix", @@ -609,23 +604,23 @@ "docLength": null }, { - "id": "3fa09193-ee9b-8ebd-e44e-8d5f121f7d5a", + "id": "1d4a0115-e749-6b95-ef46-016f982f6e14", "ancestors": [ - "cdcb03d8-8236-2faa-f349-0e323183c24c" + "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" ], - "description": "Visualizes a real-time 3D graphical representation of a G2O optimization problem using Python's Matplotlib library.", + "description": "Loads G2O files, visualizes their positions and edges in 3D, and updates the display in real-time using matplotlib.", "params": [ { "name": "optimizer", "type_name": "object", - "description": "Used to load G2O files for visualization." + "description": "Used to load a G2O file for visualization." } ], "returns": null, "usage": { "language": "python", - "code": "import numpy as np\nfrom g2o import *\nfrom g2o_visualizer import G2OVisualizer\n\n# Create a new instance of the SpecificWorker class.\nworker = SpecificWorker(proxy_map=None, startup_check=False)\n\n# Set up the visualizer and G2OGraph for the worker.\nworker.visualizer = G2OVisualizer(\"G2O Graph\")\nworker.g2o = G2OGraph(verbose=False)\n\n# Load a graph file into the worker's G2OGraph instance.\nworker.g2o.load(\"archivo.g2o\")\n\n# Set up the visualization loop for the worker.\nwhile True:\n # Load the graph file and update the visualizer.\n optimizer = worker.g2o\n positions = []\n for vertex_id in range(optimizer.vertices().size()):\n vertex = optimizer.vertex(vertex_id)\n position = vertex.estimate()\n positions.append(position)\n positions = np.array(positions)\n\n worker.visualizer.ax.clear()\n worker.visualizer.ax.scatter(positions[:, 0], positions[:, 1], positions[:, 2], c='b', marker='o', label='Vertices')\n\n edges = optimizer.edges()\n for edge_id in range(edges.size()):\n edge = edges[edge_id]\n measurement = edge.measurement()\n worker.visualizer.ax.plot(measurement[:, 0], measurement[:, 1], measurement[:, 2], c='r')\n\n worker.visualizer.ax.set_xlabel('X')\n worker.visualizer.ax.set_ylabel('Y')\n worker.visualizer.ax.set_zlabel('Z')\n worker.visualizer.ax.legend()\n\n worker.visualizer.draw()\n worker.visualizer.pause(0.1)\n", - "description": "" + "code": "import matplotlib.pyplot as plt\nfrom matplotlib import animation\n\n# Create a new SpecificWorker instance\nspecific_worker = SpecificWorker(proxy_map, startup_check=False)\n\n# Load the G2O graph data from a file\noptimizer = specific_worker.g2o.load(\"archivo.g2o\")\n\n# Visualize the G2O graph in real-time using matplotlib\nanim = animation.FuncAnimation(plt.figure(), specific_worker.visualize_g2o_realtime, fargs=(optimizer,))\n", + "description": "\nThe user creates a new instance of the SpecificWorker class, which is a subclass of GenericWorker. Then, they load the G2O graph data from a file using the g2o.load() method and pass it as an argument to the visualize_g2o_realtime function. The function will create a 3D scatter plot of the vertices in the graph using matplotlib's animation module." }, "name": "visualize_g2o_realtime", "location": { @@ -640,28 +635,28 @@ "docLength": null }, { - "id": "595b6b91-9394-1b80-7345-230aff72914c", + "id": "8e143288-7f47-9db0-dd4d-4eae4da6c637", "ancestors": [ - "cdcb03d8-8236-2faa-f349-0e323183c24c" + "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" ], - "description": "Updates attributes of a node in a graph based on the current time and robot state, and appends the updated values to an odometry queue for later use.", + "description": "Updates the attributes of a node in a graph, specifically the odometry node, and appends a new entry to the odometry queue based on the current robot position and speed.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to identify the node for which the attributes are being updated." + "description": "Used to identify the node for which attributes are being updated, specifically the odometry node." }, { "name": "attribute_names", "type_name": "[str]", - "description": "An array of strings representing the names of attributes to be updated on the given node ID." + "description": "An array of strings that represents the names of attributes to be updated on a node in the graph." } ], "returns": null, "usage": { "language": "python", - "code": "specificWorker = SpecificWorker(proxy_map)\nspecificWorker.update_node_att(id=20, attribute_names=[\"robot_current_advance_speed\", \"robot_current_side_speed\", \"-robot_current_angular_speed\"])\n", - "description": "\nIn this example, the end-user is creating a SpecificWorker instance and calling update_node_att with an ID of 20 and a list of attribute names. The function will retrieve the node from the graph based on the ID provided and update the specified attributes in the node's attribute dictionary." + "code": "worker = SpecificWorker(proxy_map=proxy_map)\n# Update the attribute of a node in the graph.\nworker.update_node_att(id=20, attribute_names=[\"robot_current_advance_speed\", \"robot_current_side_speed\"])\n", + "description": "\nThis code would update the attributes of the node with id 20 in the graph, which is a G2OGraph. The function updates the values of the attributes \"robot_current_advance_speed\" and \"robot_current_side_speed\"." }, "name": "update_node_att", "location": { @@ -676,28 +671,28 @@ "docLength": null }, { - "id": "e4669eb8-bce7-27aa-de44-d04a35cedb5c", + "id": "92490404-1b41-4ab7-2044-f62b24cc8d3e", "ancestors": [ - "cdcb03d8-8236-2faa-f349-0e323183c24c" + "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" ], - "description": "Updates the node with the given ID based on the type parameter. If the type is \"corner\", it initializes a room graph and sets the `init_graph` attribute to `True`.", + "description": "Updates an individual node's type, specifically \"corner\". If the node belongs to a room, it initializes the graph's room structure if necessary.", "params": [ { "name": "id", "type_name": "int", - "description": "An identifier for the node to be updated." + "description": "Used to identify the node being updated." }, { "name": "type", "type_name": "str", - "description": "Used to specify the node's type, which can be either \"corner\" or any other valid value." + "description": "Used to identify the node's type, which can be either \"corner\" or \"room\"." } ], "returns": null, "usage": { "language": "python", - "code": "worker = SpecificWorker(proxy_map) # Initializes a worker with the proxy map\nid = 5 # Assigns node ID to the variable id\ntype = \"corner\" # Assigns node type to the variable type\nworker.update_node(id, type) # Updates the node in the graph with the given ID and type\n", - "description": "\nIn this example, the worker is initialized by passing the proxy map as a parameter and then the update_node function is called with the id and type arguments assigned to variables outside the function." + "code": "specific_worker = SpecificWorker(\"proxy_map\")\nnode_id = 10 # arbitrary node ID\nnode_type = \"corner\" # arbitrary node type string\nspecific_worker.update_node(node_id, node_type)\n", + "description": "\nThis example shows how the update_node method of the SpecificWorker class might be used to update a node in the graph with the specified node ID and node type. The function takes two arguments: the first is an integer representing the node ID, and the second is a string representing the node type. In this case, we have set the node ID to 10 and the node type to \"corner\", but these values can be any valid integers or strings that correspond to actual nodes in the graph." }, "name": "update_node", "location": { @@ -712,23 +707,23 @@ "docLength": null }, { - "id": "6e50ec45-7089-e09c-a842-9c0f0714d216", + "id": "9d7aca63-132b-0493-8e40-0f29a33de3d0", "ancestors": [ - "cdcb03d8-8236-2faa-f349-0e323183c24c" + "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" ], - "description": "Deletes a node from a list of nodes maintained by the worker.", + "description": "Deletes a node from a graph data structure represented in the method signature, by setting the `room_initialized` attribute to `False`.", "params": [ { "name": "id", "type_name": "int", - "description": "Used to represent the unique identifier of the node to be deleted." + "description": "Intended to represent the unique identifier of the node to be deleted." } ], "returns": null, "usage": { "language": "python", - "code": "# Create an instance of the SpecificWorker class\nworker = SpecificWorker(proxy_map)\n\n# Call the delete_node method with the ID of the node to delete\nworker.delete_node(1234567890)\n", - "description": "\nIn this example, we create an instance of the SpecificWorker class and then call the delete_node method with the ID of the node we want to delete (1234567890). The function will then execute the code to delete the specified node from the graph." + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\n# ... (code for worker initialization)\nworker.delete_node(1234)\n", + "description": "\nIn this example, the user creates a specific worker instance using the provided proxy map and sets `startup_check` to False. Then they call the `delete_node()` function with node ID 1234 as an argument, which deletes the specified node from the graph represented by the worker's G2OGraph object." }, "name": "delete_node", "location": { @@ -743,21 +738,21 @@ "docLength": null }, { - "id": "67728d31-113e-e0b4-c34d-aaa4fd6ab27c", + "id": "1fbca7ab-3716-df8e-d44a-d5010fbc319e", "ancestors": [ - "cdcb03d8-8236-2faa-f349-0e323183c24c" + "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" ], - "description": "Updates the room ID and RT translation and rotation values based on the incoming edge type and node attributes.", + "description": "Updates the room ID and sets the current edge set based on node type and RT edge information.", "params": [ { "name": "fr", "type_name": "int", - "description": "The index of the current edge being processed." + "description": "Representing the from node ID in the graph." }, { "name": "to", "type_name": "int", - "description": "Representing the index of the next node in the graph that is being processed." + "description": "Used to represent the ID of the node that follows the edge being updated." }, { "name": "type", @@ -768,8 +763,8 @@ "returns": null, "usage": { "language": "python", - "code": "# In the context of the SpecificWorker class, where \"worker\" is an instance of the SpecificWorker class\nworker.update_edge(fr=0, to=1, type=\"current\") # Update the current edge from node 0 to node 1\nworker.update_edge(fr=0, to=1, type=\"RT\") # Update the RT edge from node 0 to node 1\n", - "description": "\nThe update_edge function updates the edge between two nodes in the DSRGraph with a specific type (current or RT). The fr parameter specifies the start node of the edge, while the to parameter specifies the end node." + "code": "from SpecificWorker import SpecificWorker\nimport time\n\n# Create a new instance of the class SpecificWorker with proxy_map as a parameter.\nworker = SpecificWorker(proxy_map)\n\n# Update the edge with from node fr and to node to.\nworker.update_edge(fr, to, \"current\")\n", + "description": "\nIn this example, the SpecificWorker object is created using the proxy_map argument and then the update_edge method of the class is called using the nodes fr and to as parameters. The type parameter is set to \"current\" indicating that the edge represents a current edge in the graph." }, "name": "update_edge", "location": { @@ -784,33 +779,33 @@ "docLength": null }, { - "id": "4825b582-5f59-328e-c944-7eddbc75d3f0", + "id": "9020eb51-763c-97aa-9b4c-b9312291e281", "ancestors": [ - "cdcb03d8-8236-2faa-f349-0e323183c24c" + "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" ], - "description": "Deletes an edge from a graph, identified by its index (fr) and type (to).", + "description": "Deletes an edge from a graph, specified by its index (fr), type (to), and the worker instance.", "params": [ { "name": "fr", "type_name": "int", - "description": "0-based index of the edge to be deleted." + "description": "1st in the function signature, indicating that it should be the first argument passed to the function when calling it." }, { "name": "to", "type_name": "int", - "description": "Used to specify the target vertex for edge deletion." + "description": "Representing the second vertex index of an edge to be deleted." }, { "name": "type", "type_name": "str", - "description": "Used to specify the type of edge to delete, with possible values 'direct' or 'indirect'." + "description": "Used to specify the edge type to be deleted, which can be either \"weighted\" or \"unweighted\"." } ], "returns": null, "usage": { "language": "python", - "code": "my_graph = SpecificWorker(proxy_map)\n# Create a new edge between two nodes in the graph\nmy_graph.add_edge(0, 1, \"type\")\n# Delete the edge from the graph\nmy_graph.delete_edge(0, 1, \"type\")\n", - "description": "" + "code": "from SpecificWorker import GenericWorker\n\n# Create a new instance of the SpecificWorker class\nworker = SpecificWorker(proxy_map)\n\n# Add an edge to the graph\nworker.add_edge(0, 1, \"type\")\n\n# Delete an edge from the graph\nworker.delete_edge(0, 1, \"type\")\n", + "description": "\nThis example demonstrates how the delete_edge function is used to remove a previously created edge between nodes with IDs 0 and 1 of type \"type\" from the graph." }, "name": "delete_edge", "location": { diff --git a/.komment/komment.json b/.komment/komment.json index 822afa63..d789db51 100644 --- a/.komment/komment.json +++ b/.komment/komment.json @@ -1,7 +1,7 @@ { "meta": { "version": "1", - "updated_at": "2024-07-17T07:58:01.192Z", + "updated_at": "2024-07-19T07:43:08.491Z", "created_at": "2024-07-03T15:48:27.349Z", "pipelines": [ "50c96b7c-ad3b-4b93-ae19-7f6fbd5637f7", @@ -11,7 +11,8 @@ "d6a72e5c-096b-4179-ac39-3343ea5ec3a5", "395b32ba-01b1-45bb-84e0-96a1b0abb44e", "fb0c09b8-650c-42fb-a814-0c07d121ec67", - "3216d745-95dc-419d-8524-71b95ba17b45" + "3216d745-95dc-419d-8524-71b95ba17b45", + "bdcf9dc7-844d-44d2-af0f-6fcd7dfa4a7c" ] }, "lookup": [ diff --git a/agents/g2o_agent/src/specificworker.py b/agents/g2o_agent/src/specificworker.py index ae801187..7b23b755 100644 --- a/agents/g2o_agent/src/specificworker.py +++ b/agents/g2o_agent/src/specificworker.py @@ -49,129 +49,126 @@ class SpecificWorker(GenericWorker): """ - Manages a specific worker's tasks and updates in a graph. It has methods for - updating node attributes, deleting nodes, and setting up RT transmission, as - well as checking for updates and handling errors. + Manages a graph-based representation of an environment and its objects, providing + functions for updating node and edge attributes, deleting nodes and edges, and + handling RT messages. It also maintains a set of rooms and their initial positions. Attributes: - Period (float|int): Used to control the update rate of the graph. It - determines how often the graph is updated, with a higher value resulting - in more frequent updates. - agent_id (int|str): Used to store the ID of the agent that will be using - the specific worker. - g (Graph|QGraphicsScene): Used to store the graph data for the worker's - algorithm, including the nodes, edges, and their attributes. + Period (float|int): Set to `200` by default, indicating the time interval + (in milliseconds) between successive calls to the `update` method. It + determines how frequently the worker updates its state. + agent_id (int|str): Used to identify the worker's agent in the simulation + environment. + g (Graph|NetworkXGraph): Used for managing graphs in the worker's update + and delete operations. startup_check (QTimersingleShot200,QApplicationinstancequit): Used to check - if the application is still running after a certain time period (200 - milliseconds) and quit if so. - rt_api (Union[str,int]): Used to store the RT (Real-time) edge set ID for - the worker's specific room. + if the application should quit after a certain period of time. + rt_api (str|int): Used to store the last RT (Real-time) edge set id that + was received by the worker, so it can check if a new RT edge set has + been received since the last time it was checked. inner_api (Callable[[],None]): Used to define a custom API for the worker - to interact with its internal data structures and algorithms without - exposing them to the outer world. - odometry_node_id (int): Used to identify the node in the graph that - represents the robot's odometry information. - odometry_queue (List[Tuple[float,float,float,int]]): Used to store odometry - data from the environment, containing the robot's current advance - speed, side speed, and angular speed, as well as the timestamp in - milliseconds since the epoch. + to interact with its internal state and update its graph. + odometry_node_id (int): Used to store the ID of the node representing the + shadow robot in the graph, which is used for updating the robot's odometry. + odometry_queue (List[Tuple[float,float,float,int]]): Used to store the + odometry data points received from the ROS node. It is updated every + 200 milliseconds. last_odometry (float|List[float]): Used to store the last known odometry - values (position, orientation, and translation) of the robot, which - are updated in real-time during the simulation. - g2o (Graph|Tuple[str,str]): Used to represent the graph structure of the - robot's environment. It contains a tuple of two elements: the first - element is the name of the graph file, and the second element is the - name of the node file. - visualizer (Callable[[QWidget],None]): Used to create a QWidget instance - for visualizing the robot's movement and orientation in real-time. - odometry_noise_std_dev (float|int): 0.1 by default, representing the - standard deviation of noise added to the odometry measurements for the - robot's position, orientation, and angular velocity. - odometry_noise_angle_std_dev (float|double): 0.2 by default, representing - the standard deviation of angle noise in the odometry estimates. It - affects how much the worker's estimate of the agent's position and - orientation deviates from the true values due to random noise in the - sensor readings. - measurement_noise_std_dev (float|int): 0.1 by default, indicating the - standard deviation of the noise present in the robot's measurements. - last_room_id (int|str): Used to store the room ID of the previous room - that the agent has entered before changing rooms, which is useful for - determining when the agent has moved to a new room. - actual_room_id (int|str): Used to store the current room ID of the agent, - which is updated when the agent moves to a new room. - elapsed (float|int): Used to store the time since the last call to the - `update()` method, which is used to update the node's attributes. - room_initialized (bool): Set to False when a room change is detected, - indicating that the worker has not yet initialized its current room's - graph. It is reset to True once the worker has successfully initialized - the new room's graph. - iterations (int|float): Used to keep track of the number of iterations - (i.e., frames) that the worker has processed. It is updated with each - frame processed by the worker, and can be used to control the speed - of the worker or to implement various algorithms. - hide (bool): Used to indicate whether the worker should be hidden or not, - affecting its visibility in the graph. - init_graph (bool): Used to indicate whether the graph has been initialized - or not. It is set to True when the worker is initialized and False - otherwise, allowing for proper handling of the graph and its nodes and - edges in the update methods. - current_edge_set (bool): Used to track whether an edge set has been computed - for the current room id. It is set to True when the first RT edge set - is computed and False otherwise, allowing the worker to avoid recomputing - the edge set if it has already been computed for the same room id. - first_rt_set (bool): Set to `True` when the RT translation and rotation - values are being set for the first time, and `False` otherwise. - translation_to_set (float|List[float]): Used to store the translation - values set by the RT algorithm. - rotation_to_set (float|int): A value representing the rotation of the robot - to set its end effector at a specific location. - room_polygon (List[float]): Used to store the vertices of a polygon that - represents the room boundary in the environment. - security_polygon (Union[int,List[float]]): Used to store the security - polygon of the worker's workspace in 3D space. It is used to detect - collisions between the worker's manipulator and objects in the environment. - initialize_g2o_graph (void): Used to create a new graph for the given - worker instance, which will be used to store the robot's position and - orientation in the environment. - rt_set_last_time (int|float): Used to store the last time a RT (Real-Time) - edge was set for a specific agent ID. It is used in conjunction with - the `rt_time_min` attribute to determine when to reset the RT translation - and rotation values. - rt_time_min (float): Defined as the minimum time duration between two - consecutive RT sets in milliseconds. It is used to determine when to - reset the translation and rotation values for the shadow agent during - RT tracking. + values (position, orientation, and advance speed) for the robot. + g2o (None|Graph2O): Used to store the graph data in Graph2O format for the + robot's environment. + odometry_noise_std_dev (float|int): Used to control the noise level in + robot odometry readings. + odometry_noise_angle_std_dev (float|int): 0.1 by default, representing the + standard deviation of the angle noise added to the robot's odometry + measurements during simulation. + measurement_noise_std_dev (float|double): Used to represent the standard + deviation of measurement noise in the worker's measurements. It is + used to simulate random fluctuations in the measurements during training. + last_room_id (int|str): Used to store the last room ID seen by the worker + before changing rooms, which allows the worker to track the current + room ID. + actual_room_id (str|int): Used to keep track of the current room ID that + the worker is in, during its execution + elapsed (float|int): Used to store the elapsed time since the worker's + last call to the `update` method, which can be used to control the + worker's execution rate. + room_initialized (bool): Used to track whether a specific room has been + initialized for RT mapping. It is set to False when the room is first + encountered, and True when it has been successfully mapped with the + robot's pose. + iterations (int|float): Used to keep track of the number of iterations of + the worker's tasks that have been performed. + hide (bool): Used to hide or show the worker's updates during simulation. + init_graph (bool): Set to True when the worker initializes its graph and + False otherwise, indicating whether the graph has been initialized or + not. + current_edge_set (bool): Used to keep track of whether the current edge + set has been updated recently, indicating when a new RT translation + or rotation should be computed. It is set to True after each edge set + update and reset to False after a certain time interval (defined as + `rt_time_min` in the code) has passed without any updates, to avoid + computing unnecessary RT translations or rotations. + first_rt_set (bool): Set to True when the RT translation and rotation are + first detected, False otherwise. It indicates whether the RT set has + been detected for a given edge. + translation_to_set (float|List[float]): Used to store the translation of + the robot to set. It is updated when the RT transmission is received + and its value represents the distance from the origin of the robot's + frame to the origin of the set frame in the global coordinate system. + rotation_to_set (float|List[float]): Used to store the Euler angles + representing the rotation from the current room to the target set. + room_polygon (List[float]): Used to store the coordinates of a polygon + representing the boundary of a room in the environment, which is used + for collision detection and avoidance. + security_polygon (Shape|str): Used to store a polygon that defines the + security area around the robot, which is used for collision detection + and obstacle avoidance. + initialize_g2o_graph (void): Used to initialize a Graph2O graph, which is + a data structure representing a robot's environment, and perform other + operations such as adding nodes and edges, setting attributes, and + updating node positions. + rt_set_last_time (float|int): Used to track the time since the last RT set + was created by the worker. It is used to determine when to set a new + RT translation and rotation value. + rt_time_min (float|int): Defined as the minimum time interval between + consecutive RT sets. Its purpose is to ensure that the robot's motion + is smooth and does not oscillate excessively during navigation. last_update_with_corners (int|bool): Used to keep track of the last time - a node or edge update was performed with corner information. It is - initially set to False, and its value changes when updates are performed - with corners. Its purpose is to check if the worker has updated with - corners before, so as to avoid unnecessary updates in the future. - timer (float|int): Used to schedule a call to the `QApplication.instance().quit()` - function after 2 seconds. - compute (Callable[[],Any]): Used to define a function that computes the - worker's task based on the graph and other attributes. - update_node_att (Callable[int,[str]]): Used to update the attributes of a - specific node in the graph based on its ID and the type of update. It - takes two arguments: id, which is the ID of the node to be updated, - and attribute_names, which is a list of attribute names to be updated. - update_edge (Callable[int,str,str]): Used to update the attributes of an - edge in the graph based on its type (current or RT translation) and - the node it connects. - update_edge_att (Tuple[str,]): Used to update the attributes of an edge - in a graph based on its type and the name of the attribute. + corners were updated. When the corner of a room was updated, it's set + to True, otherwise it's set to False. + timer (QTimersingleShot200,QApplicationinstancequit): Used to schedule a + call to the `QApplication.instance().quit()` function after 200 + milliseconds. This allows the worker to run indefinitely until the + user presses the "Stop" button. + compute (Callable[[],float]): Used to compute the next node id to update + based on the current node id and the edge id. It takes no arguments + and returns a floating-point value representing the next node id to update. + update_node_att (List[str]): Used to update the attributes of a node in + the graph based on certain conditions, such as when the node's id + matches the `odometry_node_id` constant. + update_edge (Callable[[int,int,str],None]): Called when an edge's type + changes to "RT". It sets the `translation_to_set`, `rotation_to_set`, + and `rt_set_last_time` attributes based on the new RT translation and + rotation. + update_edge_att (List[str]): Used to update the attributes of an edge based + on a specific condition. It takes three parameters: `fr`, `to`, and + `type`, which are the ID of the edge being updated, its destination + ID, and the type of update respectively. """ def __init__(self, proxy_map, startup_check=False): """ - Initializes instance variables and sets up event handling for signals - related to node and edge attributes, as well as a timer to compute the - worker's output every `Period` seconds. + Initializes various components of the worker, including the graph, odometry + queue, and timer. It also sets up signal connections for updates on nodes + and edges. Args: - proxy_map (Dict[str, Any]): Used to store the mapping between the - original node attributes and their proxies. - startup_check (bool): Used to check if the graph has already been - initialized before starting the worker's execution. + proxy_map (Dict[str, Any]): Used to store map-related information, + such as the graph's nodes and edges, which are required for the + worker's functionality. + startup_check (bool): Used to check if the graph has been properly + initialized before proceeding with the worker's tasks. """ super(SpecificWorker, self).__init__(proxy_map) @@ -192,7 +189,7 @@ def __init__(self, proxy_map, startup_check=False): self.last_odometry = None # Initialize g2o graph with visualizer self.g2o = G2OGraph(verbose=False) - self.visualizer = G2OVisualizer("G2O Graph") + # self.visualizer = G2OVisualizer("G2O Graph") self.odometry_noise_std_dev = 1 # Standard deviation for odometry noise self.odometry_noise_angle_std_dev = 1 # Standard deviation for odometry noise @@ -248,8 +245,12 @@ def setParams(self, params): @QtCore.Slot() def compute(self): """ - Computes and updates the robot's position, orientation, and covariance - matrix based on RT messages received from the environment. + Performs the following tasks: + 1/ Checks if enough time has passed since the last frame was computed. + 2/ Updates the robot's position and orientation using odometry data. + 3/ Generates a new RT edge for the robot based on its current position and + orientation. + 4/ Adds the new RT edge to the graph. """ if time.time() - self.elapsed > 1: @@ -341,7 +342,7 @@ def compute(self): # 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) + # self.visualizer.update_graph(self.g2o) # 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] @@ -384,14 +385,13 @@ def initialize_g2o_graph(self): # print("Initializing g2o graph") # get robot pose in room """ - Initializes a Graph-Based Odometry (G2O) graph for a specific room by: - 1/ Retrieving node and edge information from a robot's sensor data. - 2/ Filtering out invalid nodes and edges based on their position and orientation. - 3/ Adding nominal corners and fixed poses to the G2O graph using the - retrieved data. + 1) retrieves nodes and edges from the ROS topic, 2) validates node and + edge presence, 3) adds nominal corners and fixed poses to the G2O graph + for robot and door nodes, and 4) initializes last odometry timestamp. Returns: - bool: True if the g2o graph is successfully initialized and False otherwise. + bool: 1 if the initialization of the G2O graph was successful, and 0 + otherwise. """ self.g2o.clear_graph() @@ -598,18 +598,16 @@ def initialize_g2o_graph(self): def get_displacement(self, odometry): """ - Computes the displacement of a robot based on its odometry data, taking - into account the advance, lateral movement, and angular velocity of the robot. + Calculates the displacement of a robot based on its odometry data, computing + the lateral displacement, forward displacement, and angular displacement. Args: - odometry (Tuple[float, float, float]): An immutable sequence containing - the robot's odometry data at each time step, consisting of the - linear displacement, lateral displacement, and angular velocity, - respectively. + odometry (Tuple[float, float, float]): A sequence of 3-element tuples + representing the robot's position, velocity, and timestamp. Returns: - Tuple[float,float,float]: The displacement along the x, y and z axes, - respectively, calculated using the odometry data from a robot's sensors. + Tuple[float,float,float]: 3 components of displacement (lateral, avance + and angular) calculated based on the odometry data. """ desplazamiento_avance = 0 @@ -632,17 +630,15 @@ def get_displacement(self, odometry): def get_covariance_matrix(self, vertex): """ - Computes the covariance matrix of a set of vertices in a graph, using an - optimization algorithm to compute the marginals of the vertices and then - constructing the covariance matrix from the resulting upper triangle. + Computes the covariance matrix of a set of vertices in a graph using the + G2O optimizer and returns the result. Args: - vertex (G2O.Vertex | G2O.HessianIndex): Used to compute the covariance - matrix for a specific vertex in a graph. + vertex (G2O.HessianIndex): Used to represent a vertex in the graph. Returns: - Tuple[bool,npndarray]: A result of computing covariance matrix and the - actual computed matrix. + Tuple[bool,npndarray]: A result of computing marginals and a covariance + matrix. """ cov_vertices = [(vertex.hessian_index(), vertex.hessian_index())] @@ -659,11 +655,11 @@ def get_covariance_matrix(self, vertex): def visualize_g2o_realtime(self, optimizer): """ - Visualizes a real-time 3D graphical representation of a G2O optimization - problem using Python's Matplotlib library. + Loads G2O files, visualizes their positions and edges in 3D, and updates + the display in real-time using matplotlib. Args: - optimizer (object): Used to load G2O files for visualization. + optimizer (object): Used to load a G2O file for visualization. """ plt.ion() @@ -712,14 +708,15 @@ def update_node_att(self, id: int, attribute_names: [str]): # print("INIT GRAPH") """ - Updates attributes of a node in a graph based on the current time and robot - state, and appends the updated values to an odometry queue for later use. + Updates the attributes of a node in a graph, specifically the odometry + node, and appends a new entry to the odometry queue based on the current + robot position and speed. Args: - id (int): Used to identify the node for which the attributes are being - updated. - attribute_names ([str]): An array of strings representing the names - of attributes to be updated on the given node ID. + id (int): Used to identify the node for which attributes are being + updated, specifically the odometry node. + attribute_names ([str]): An array of strings that represents the names + of attributes to be updated on a node in the graph. """ if id == self.odometry_node_id: @@ -740,24 +737,25 @@ def update_node(self, id: int, type: str): # self.room_initialized = True if self.initialize_g2o_graph() else False # self.init_graph = True """ - Updates the node with the given ID based on the type parameter. If the - type is "corner", it initializes a room graph and sets the `init_graph` - attribute to `True`. + Updates an individual node's type, specifically "corner". If the node + belongs to a room, it initializes the graph's room structure if necessary. Args: - id (int): An identifier for the node to be updated. - type (str): Used to specify the node's type, which can be either - "corner" or any other valid value. + id (int): Used to identify the node being updated. + type (str): Used to identify the node's type, which can be either + "corner" or "room". """ pass def delete_node(self, id: int): """ - Deletes a node from a list of nodes maintained by the worker. + Deletes a node from a graph data structure represented in the method + signature, by setting the `room_initialized` attribute to `False`. Args: - id (int): Used to represent the unique identifier of the node to be deleted. + id (int): Intended to represent the unique identifier of the node to + be deleted. """ pass @@ -770,13 +768,13 @@ def delete_node(self, id: int): def update_edge(self, fr: int, to: int, type: str): """ - Updates the room ID and RT translation and rotation values based on the - incoming edge type and node attributes. + Updates the room ID and sets the current edge set based on node type and + RT edge information. Args: - fr (int): The index of the current edge being processed. - to (int): Representing the index of the next node in the graph that - is being processed. + fr (int): Representing the from node ID in the graph. + to (int): Used to represent the ID of the node that follows the edge + being updated. type (str): Used to specify the edge's type, which can be either "current" or "RT". @@ -810,13 +808,15 @@ def update_edge_att(self, fr: int, to: int, type: str, attribute_names: [str]): def delete_edge(self, fr: int, to: int, type: str): """ - Deletes an edge from a graph, identified by its index (fr) and type (to). + Deletes an edge from a graph, specified by its index (fr), type (to), and + the worker instance. Args: - fr (int): 0-based index of the edge to be deleted. - to (int): Used to specify the target vertex for edge deletion. - type (str): Used to specify the type of edge to delete, with possible - values 'direct' or 'indirect'. + fr (int): 1st in the function signature, indicating that it should be + the first argument passed to the function when calling it. + to (int): Representing the second vertex index of an edge to be deleted. + type (str): Used to specify the edge type to be deleted, which can be + either "weighted" or "unweighted". """ pass From da283d605faf30602f1283331f1f4c40bf283a05 Mon Sep 17 00:00:00 2001 From: "komment-ai[bot]" <122626893+komment-ai[bot]@users.noreply.github.com> Date: Wed, 31 Jul 2024 13:11:14 +0000 Subject: [PATCH 10/10] Added comments to 39 functions across 2 files --- .komment/00000.json | 865 +++++++++--------- .komment/komment.json | 5 +- agents/g2o_agent/src/specificworker.py | 353 +++---- .../src/specificworker.py | 565 +++++++----- 4 files changed, 996 insertions(+), 792 deletions(-) diff --git a/.komment/00000.json b/.komment/00000.json index 4d292665..bd81dfec 100644 --- a/.komment/00000.json +++ b/.komment/00000.json @@ -251,187 +251,187 @@ "path": "agents/g2o_agent/src/specificworker.py", "content": { "structured": { - "description": "a class called `Robot` that interacts with a graph representing a robot's environment and its movements. The class has several methods for adding, updating, and deleting nodes and edges in the graph, as well as updating edge attributes. It also keeps track of the robot's current position and orientation, and uses this information to update its graph representation.\n\nThe code uses the `networkx` library to represent the graph, and the `time` module for timing purposes. The class has methods to initialize and update the graph, as well as to handle various events such as new nodes or edges being added, or a room changing. It also has methods to delete nodes and edges when they are no longer needed.\n\nOverall, the code is concerned with maintaining an accurate representation of the robot's environment and movements, and updating this representation in response to changes in the graph.", + "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": "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359", + "id": "609f6edc-14d7-26a1-1c4c-62744b75a643", "ancestors": [], - "description": "Manages a graph-based representation of an environment and its objects, providing functions for updating node and edge attributes, deleting nodes and edges, and handling RT messages. It also maintains a set of rooms and their initial positions.", + "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": "float|int", - "description": "Set to `200` by default, indicating the time interval (in milliseconds) between successive calls to the `update` method. It determines how frequently the worker updates its state." + "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|str", - "description": "Used to identify the worker's agent in the simulation environment." + "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": "Graph|NetworkXGraph", - "description": "Used for managing graphs in the worker's update and delete operations." + "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": "QTimersingleShot200,QApplicationinstancequit", - "description": "Used to check if the application should quit after a certain period of time." + "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": "str|int", - "description": "Used to store the last RT (Real-time) edge set id that was received by the worker, so it can check if a new RT edge set has been received since the last time it was checked." + "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": "Callable[[],None]", - "description": "Used to define a custom API for the worker to interact with its internal state and update its graph." + "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": "Used to store the ID of the node representing the shadow robot in the graph, which is used for updating the robot's odometry." + "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": "List[Tuple[float,float,float,int]]", - "description": "Used to store the odometry data points received from the ROS node. It is updated every 200 milliseconds." + "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": "float|List[float]", - "description": "Used to store the last known odometry values (position, orientation, and advance speed) for the robot." + "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": "None|Graph2O", - "description": "Used to store the graph data in Graph2O format for the robot's environment." + "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|int", - "description": "Used to control the noise level in robot odometry readings." + "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|int", - "description": "0.1 by default, representing the standard deviation of the angle noise added to the robot's odometry measurements during simulation." + "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|double", - "description": "Used to represent the standard deviation of measurement noise in the worker's measurements. It is used to simulate random fluctuations in the measurements during training." + "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": "int|str", - "description": "Used to store the last room ID seen by the worker before changing rooms, which allows the worker to track the current 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": "str|int", - "description": "Used to keep track of the current room ID that the worker is in, during its execution" + "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|int", - "description": "Used to store the elapsed time since the worker's last call to the `update` method, which can be used to control the worker's execution rate." + "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 track whether a specific room has been initialized for RT mapping. It is set to False when the room is first encountered, and True when it has been successfully mapped with the robot's pose." + "description": "Used to indicate whether the g2o graph has been successfully initialized for a specific room or not." }, { "name": "iterations", - "type_name": "int|float", - "description": "Used to keep track of the number of iterations of the worker's tasks that have been performed." + "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": "bool", - "description": "Used to hide or show the worker's updates during simulation." + "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": "Set to True when the worker initializes its graph and False otherwise, indicating whether the graph has been initialized or not." + "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": "Used to keep track of whether the current edge set has been updated recently, indicating when a new RT translation or rotation should be computed. It is set to True after each edge set update and reset to False after a certain time interval (defined as `rt_time_min` in the code) has passed without any updates, to avoid computing unnecessary RT translations or rotations." + "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 the RT translation and rotation are first detected, False otherwise. It indicates whether the RT set has been detected for a given edge." + "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": "float|List[float]", - "description": "Used to store the translation of the robot to set. It is updated when the RT transmission is received and its value represents the distance from the origin of the robot's frame to the origin of the set frame in the global coordinate system." + "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": "float|List[float]", - "description": "Used to store the Euler angles representing the rotation from the current room to the target 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": "List[float]", - "description": "Used to store the coordinates of a polygon representing the boundary of a room in the environment, which is used for collision detection and avoidance." + "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": "Shape|str", - "description": "Used to store a polygon that defines the security area around the robot, which is used for collision detection and obstacle avoidance." + "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": "void", - "description": "Used to initialize a Graph2O graph, which is a data structure representing a robot's environment, and perform other operations such as adding nodes and edges, setting attributes, and updating node positions." + "type_name": "bool", + "description": "Used to initialize a G2O graph object." }, { "name": "rt_set_last_time", - "type_name": "float|int", - "description": "Used to track the time since the last RT set was created by the worker. It is used to determine when to set a new RT translation and rotation value." + "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|int", - "description": "Defined as the minimum time interval between consecutive RT sets. Its purpose is to ensure that the robot's motion is smooth and does not oscillate excessively during navigation." + "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": "int|bool", - "description": "Used to keep track of the last time corners were updated. When the corner of a room was updated, it's set to True, otherwise it's set to False." + "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": "QTimersingleShot200,QApplicationinstancequit", - "description": "Used to schedule a call to the `QApplication.instance().quit()` function after 200 milliseconds. This allows the worker to run indefinitely until the user presses the \"Stop\" button." + "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": "Callable[[],float]", - "description": "Used to compute the next node id to update based on the current node id and the edge id. It takes no arguments and returns a floating-point value representing the next node id to update." + "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": "List[str]", - "description": "Used to update the attributes of a node in the graph based on certain conditions, such as when the node's id matches the `odometry_node_id` constant." + "type_name": "NoneNone", + "description": "Not defined within the given code snippet." }, { "name": "update_edge", - "type_name": "Callable[[int,int,str],None]", - "description": "Called when an edge's type changes to \"RT\". It sets the `translation_to_set`, `rotation_to_set`, and `rt_set_last_time` attributes based on the new RT translation and rotation." + "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": "List[str]", - "description": "Used to update the attributes of an edge based on a specific condition. It takes three parameters: `fr`, `to`, and `type`, which are the ID of the edge being updated, its destination ID, and the type of update respectively." + "type_name": "None", + "description": "Passed as a parameter to the `update_edge_att` method. It is not used anywhere in the code." } ], "name": "SpecificWorker", @@ -447,28 +447,28 @@ "docLength": null }, { - "id": "1c3f1211-7524-8695-b942-f5e904197296", + "id": "d4c86ec3-d2a9-ebbb-6b41-649a5fecc346", "ancestors": [ - "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" + "609f6edc-14d7-26a1-1c4c-62744b75a643" ], - "description": "Initializes various components of the worker, including the graph, odometry queue, and timer. It also sets up signal connections for updates on nodes and edges.", + "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": "Dict[str, Any]", - "description": "Used to store map-related information, such as the graph's nodes and edges, which are required for the worker's functionality." + "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 check if the graph has been properly initialized before proceeding with the worker's tasks." + "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": "# Initialize the class with the proxy_map and startup check parameter set to False\nworker = SpecificWorker(proxy_map, startup_check=False)\n\n# Set the agent ID and initialize the graph\nworker.agent_id = 20\nworker.initialize_graph()\n\n# Connect the signals for updating node attributes, deleting nodes, and updating edges\nsignals.connect(worker.g, signals.UPDATE_NODE_ATTR, worker.update_node_att)\nsignals.connect(worker.g, signals.DELETE_NODE, worker.delete_node)\nsignals.connect(worker.g, signals.UPDATE_EDGE, worker.update_edge)\nsignals.connect(worker.g, signals.UPDATE_EDGE_ATTR, worker.update_edge_att)\n", - "description": "\nThis code initializes the SpecificWorker class with a proxy map and sets its agent ID to 20. It then connects the signals for updating node attributes, deleting nodes, and updating edges using the signals module." + "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": { @@ -483,17 +483,17 @@ "docLength": null }, { - "id": "ee0a9d1b-9271-b39a-5e4f-64e1d858f292", + "id": "89daf63d-e190-4bac-1b45-3b07fbc03640", "ancestors": [ - "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" + "609f6edc-14d7-26a1-1c4c-62744b75a643" ], - "description": "Performs the following tasks:\n\n1. Checks if enough time has passed since the last frame was computed.\n2. Updates the robot's position and orientation using odometry data.\n3. Generates a new RT edge for the robot based on its current position and orientation.\n4. Adds the new RT edge to the graph.", + "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() # Create a new instance of the worker class\nworker.compute() # Call the compute method of the worker with no arguments\n", - "description": "\nThis code creates a new instance of the `SpecificWorker` class and calls its `compute()` method, which is expected to perform some computation using the data stored in the worker's variables. However, without knowing more about the implementation details of the worker's `compute()` method, it is difficult to provide a more detailed example of how this function might be used by an end-user." + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nwhile True:\n worker.compute()\n time.sleep(0.02)", + "description": "" }, "name": "compute", "location": { @@ -508,20 +508,20 @@ "docLength": null }, { - "id": "433924a0-c643-1c86-c947-e424e514e8fb", + "id": "e8b6c91f-187a-74af-dc4e-6c91a9381907", "ancestors": [ - "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" + "609f6edc-14d7-26a1-1c4c-62744b75a643" ], - "description": "1) retrieves nodes and edges from the ROS topic, 2) validates node and edge presence, 3) adds nominal corners and fixed poses to the G2O graph for robot and door nodes, and 4) initializes last odometry timestamp.", + "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", - "description": "1 if the initialization of the G2O graph was successful, and 0 otherwise." + "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": "# create graph and object for g2o\ng = DSRGraph(0, \"G2O_agent\", agent_id)\nspecific_worker = SpecificWorker(proxy_map, startup_check=False)\nspecific_worker.rt_api = rt_api(g)\nspecific_worker.inner_api = inner_api(g)\n\n# initialize g2o graph\nspecific_worker.initialize_g2o_graph()\n", - "description": "" + "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": { @@ -536,25 +536,25 @@ "docLength": null }, { - "id": "1a1a98ec-b136-4690-5d41-202f10833328", + "id": "7897dc05-497c-c28e-3b4f-9ed0a337ea5c", "ancestors": [ - "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" + "609f6edc-14d7-26a1-1c4c-62744b75a643" ], - "description": "Calculates the displacement of a robot based on its odometry data, computing the lateral displacement, forward displacement, and angular displacement.", + "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]", - "description": "A sequence of 3-element tuples representing the robot's position, velocity, and timestamp." + "type_name": "Tuple[float, float, float, float]", + "description": "Assumed to represent the current odometry data." } ], "returns": { "type_name": "Tuple[float,float,float]", - "description": "3 components of displacement (lateral, avance and angular) calculated based on the odometry data." + "description": "A tuple containing three float values representing the lateral displacement, advance displacement, and angular displacement respectively." }, "usage": { "language": "python", - "code": "specific_worker = SpecificWorker(proxy_map)\nodometry = (10, 20, 30, time.time())\ndesplzamiento_lateral, desplzamiento_avance, desplzamiento_angular = specific_worker.get_displacement(odometry)\nprint(\"Desplazamiento lateral:\", desplzamiento_lateral)\nprint(\"Desplazamiento avance:\", desplzamiento_avance)\nprint(\"Desplazamiento angular:\", desplzamiento_angular)\n", + "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", @@ -570,25 +570,25 @@ "docLength": null }, { - "id": "70fffc25-7e1d-52b1-f24e-5a8cb6ce551c", + "id": "5175f913-8e97-5989-554b-9ba4188aead9", "ancestors": [ - "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" + "609f6edc-14d7-26a1-1c4c-62744b75a643" ], - "description": "Computes the covariance matrix of a set of vertices in a graph using the G2O optimizer and returns the result.", + "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": "G2O.HessianIndex", - "description": "Used to represent a vertex in the graph." + "type_name": "object", + "description": "Expected to have attributes such as `hessian_index`." } ], "returns": { - "type_name": "Tuple[bool,npndarray]", - "description": "A result of computing marginals and a covariance matrix." + "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": "import numpy as np\nfrom specific_worker import SpecificWorker\n\n# Create a new instance of SpecificWorker class.\nspecific_worker = SpecificWorker(proxy_map)\n\n# Get covariance matrix for a vertex with index 0.\ncov_vertices = [(specific_worker.vertex.hessian_index(), specific_worker.vertex.hessian_index())]\ncovariances, covariances_result = specific_worker.g2o.optimizer.compute_marginals(cov_vertices)\nif covariances_result:\n print(\"Covariance computed\")\n matrix = covariances.block(specific_worker.vertex.hessian_index(), specific_worker.vertex.hessian_index())\n upper_triangle = np.triu(matrix) \n print(\"Covariance matrix\", upper_triangle)\nelse:\n print(\"Covariance not computed\")\n", + "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", @@ -604,23 +604,23 @@ "docLength": null }, { - "id": "1d4a0115-e749-6b95-ef46-016f982f6e14", + "id": "0845024e-0656-4daa-114c-2c7c98f50930", "ancestors": [ - "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" + "609f6edc-14d7-26a1-1c4c-62744b75a643" ], - "description": "Loads G2O files, visualizes their positions and edges in 3D, and updates the display in real-time using matplotlib.", + "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": "object", - "description": "Used to load a G2O file for visualization." + "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": "import matplotlib.pyplot as plt\nfrom matplotlib import animation\n\n# Create a new SpecificWorker instance\nspecific_worker = SpecificWorker(proxy_map, startup_check=False)\n\n# Load the G2O graph data from a file\noptimizer = specific_worker.g2o.load(\"archivo.g2o\")\n\n# Visualize the G2O graph in real-time using matplotlib\nanim = animation.FuncAnimation(plt.figure(), specific_worker.visualize_g2o_realtime, fargs=(optimizer,))\n", - "description": "\nThe user creates a new instance of the SpecificWorker class, which is a subclass of GenericWorker. Then, they load the G2O graph data from a file using the g2o.load() method and pass it as an argument to the visualize_g2o_realtime function. The function will create a 3D scatter plot of the vertices in the graph using matplotlib's animation module." + "code": "worker = SpecificWorker(proxy_map)\nwhile True:\n worker.visualize_g2o_realtime(worker.optimizer)", + "description": "" }, "name": "visualize_g2o_realtime", "location": { @@ -635,28 +635,28 @@ "docLength": null }, { - "id": "8e143288-7f47-9db0-dd4d-4eae4da6c637", + "id": "86df470e-4c72-b08b-6b41-4ed9860faf95", "ancestors": [ - "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" + "609f6edc-14d7-26a1-1c4c-62744b75a643" ], - "description": "Updates the attributes of a node in a graph, specifically the odometry node, and appends a new entry to the odometry queue based on the current robot position and speed.", + "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": "Used to identify the node for which attributes are being updated, specifically the odometry node." + "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": "An array of strings that represents the names of attributes to be updated on a node in the graph." + "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=proxy_map)\n# Update the attribute of a node in the graph.\nworker.update_node_att(id=20, attribute_names=[\"robot_current_advance_speed\", \"robot_current_side_speed\"])\n", - "description": "\nThis code would update the attributes of the node with id 20 in the graph, which is a G2OGraph. The function updates the values of the attributes \"robot_current_advance_speed\" and \"robot_current_side_speed\"." + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nworker.update_node_att(1, [\"attribute1\", \"attribute2\"])", + "description": "" }, "name": "update_node_att", "location": { @@ -671,28 +671,28 @@ "docLength": null }, { - "id": "92490404-1b41-4ab7-2044-f62b24cc8d3e", + "id": "110e6d65-7753-3797-b145-9550aa15c08c", "ancestors": [ - "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" + "609f6edc-14d7-26a1-1c4c-62744b75a643" ], - "description": "Updates an individual node's type, specifically \"corner\". If the node belongs to a room, it initializes the graph's room structure if necessary.", + "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": "Used to identify the node being updated." + "description": "Required. It specifies the unique identifier for the node to be updated in the graph." }, { "name": "type", "type_name": "str", - "description": "Used to identify the node's type, which can be either \"corner\" or \"room\"." + "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": "specific_worker = SpecificWorker(\"proxy_map\")\nnode_id = 10 # arbitrary node ID\nnode_type = \"corner\" # arbitrary node type string\nspecific_worker.update_node(node_id, node_type)\n", - "description": "\nThis example shows how the update_node method of the SpecificWorker class might be used to update a node in the graph with the specified node ID and node type. The function takes two arguments: the first is an integer representing the node ID, and the second is a string representing the node type. In this case, we have set the node ID to 10 and the node type to \"corner\", but these values can be any valid integers or strings that correspond to actual nodes in the graph." + "code": "worker = SpecificWorker(proxy_map)\nworker.update_node(1, \"room\")", + "description": "" }, "name": "update_node", "location": { @@ -707,23 +707,23 @@ "docLength": null }, { - "id": "9d7aca63-132b-0493-8e40-0f29a33de3d0", + "id": "e9952f42-2b9e-63ab-1849-1cb653ec4423", "ancestors": [ - "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" + "609f6edc-14d7-26a1-1c4c-62744b75a643" ], - "description": "Deletes a node from a graph data structure represented in the method signature, by setting the `room_initialized` attribute to `False`.", + "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": "Intended to represent the unique identifier of the node to be deleted." + "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=False)\n# ... (code for worker initialization)\nworker.delete_node(1234)\n", - "description": "\nIn this example, the user creates a specific worker instance using the provided proxy map and sets `startup_check` to False. Then they call the `delete_node()` function with node ID 1234 as an argument, which deletes the specified node from the graph represented by the worker's G2OGraph object." + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nnode = self.g.get_node(id)\nworker.delete_node(node.id)", + "description": "" }, "name": "delete_node", "location": { @@ -738,33 +738,33 @@ "docLength": null }, { - "id": "1fbca7ab-3716-df8e-d44a-d5010fbc319e", + "id": "984f9f40-6985-028f-ea43-2b448d8b12be", "ancestors": [ - "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" + "609f6edc-14d7-26a1-1c4c-62744b75a643" ], - "description": "Updates the room ID and sets the current edge set based on node type and RT edge information.", + "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": "Representing the from node ID in the graph." + "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 to represent the ID of the node that follows the edge being updated." + "description": "Used as an edge destination node ID when updating edges of graph `g`." }, { "name": "type", "type_name": "str", - "description": "Used to specify the edge's type, which can be either \"current\" or \"RT\"." + "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": "from SpecificWorker import SpecificWorker\nimport time\n\n# Create a new instance of the class SpecificWorker with proxy_map as a parameter.\nworker = SpecificWorker(proxy_map)\n\n# Update the edge with from node fr and to node to.\nworker.update_edge(fr, to, \"current\")\n", - "description": "\nIn this example, the SpecificWorker object is created using the proxy_map argument and then the update_edge method of the class is called using the nodes fr and to as parameters. The type parameter is set to \"current\" indicating that the edge represents a current edge in the graph." + "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": { @@ -779,33 +779,33 @@ "docLength": null }, { - "id": "9020eb51-763c-97aa-9b4c-b9312291e281", + "id": "6e6d2faf-e905-d2a0-df4b-fc5568d9863a", "ancestors": [ - "f9d0a7c6-bb7a-81a5-5c42-4fecec81e359" + "609f6edc-14d7-26a1-1c4c-62744b75a643" ], - "description": "Deletes an edge from a graph, specified by its index (fr), type (to), and the worker instance.", + "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": "1st in the function signature, indicating that it should be the first argument passed to the function when calling it." + "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": "Representing the second vertex index of an edge to be deleted." + "description": "Likely an identifier for a node or vertex in a graph." }, { "name": "type", "type_name": "str", - "description": "Used to specify the edge type to be deleted, which can be either \"weighted\" or \"unweighted\"." + "description": "Intended to specify the type or nature of the edge being deleted between nodes fr and to." } ], "returns": null, "usage": { "language": "python", - "code": "from SpecificWorker import GenericWorker\n\n# Create a new instance of the SpecificWorker class\nworker = SpecificWorker(proxy_map)\n\n# Add an edge to the graph\nworker.add_edge(0, 1, \"type\")\n\n# Delete an edge from the graph\nworker.delete_edge(0, 1, \"type\")\n", - "description": "\nThis example demonstrates how the delete_edge function is used to remove a previously created edge between nodes with IDs 0 and 1 of type \"type\" from the graph." + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nworker.delete_edge(1, 2, \"edge_type\")", + "description": "" }, "name": "delete_edge", "location": { @@ -1223,203 +1223,213 @@ "path": "agents/long_term_spatial_memory_agent/src/specificworker.py", "content": { "structured": { - "description": "An igraph wrapper around a graph and provides several methods for working with it, including inserting new nodes and edges, updating edge attributes, and deleting nodes and edges. The code uses the `igraph` package to interface with the graph. Specifically, it creates a long-term graph (LTG) and a short-term graph (STG), allowing for efficient insertion, deletion, and manipulation of nodes and edges in the LTG while maintaining the integrity of the STG. The code also provides functions to update edge attributes and delete nodes or edges from the graph.", + "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": "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b", + "id": "895186b9-f902-39ab-ec46-de7001893323", "ancestors": [], - "description": "Manages a specific node and edge attributes and performs various operations on them, such as updating, inserting, deleting, and attributing. It also keeps track of the current state of the robot and updates the graph accordingly.", + "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": "Union[int,str]", - "description": "Used to store the period of time during which the worker can perform its task, such as a day or a week." + "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|str", - "description": "Used to identify the specific worker instance, allowing the agent to perform tasks and maintain a unique identity during its lifetime." + "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 represent the long-term graph of the environment, which is updated periodically by the worker." + "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": "bool|str", - "description": "Used to check if the worker has been started or not, with possible values \"yes\", \"no\", or False." + "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": "str|int", - "description": "Used for storing the RT (Real-Time) API ID which represents the current task being executed by the worker." + "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": "Dict[str,Any]", - "description": "Used to store the inner API of the worker, which is a Python function that performs actions related to the worker's task." + "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|int", - "description": "Used to store the name of the robot that will perform a specific task." + "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": "Used to identify the robot node in the graph, which is the starting point for the worker's movement." + "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": "Tuple[float,float,float]", - "description": "Stored as the last known pose (position, orientation, and scale) of the robot before any actions were taken in the environment." + "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": "str|int", - "description": "A reference to the pose of the robot when it exits a room. It is used to determine when the robot has reached its goal in a task." + "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|int", - "description": "Used to keep track of the current state of the worker, which can be either \"idle\", \"crossed\", or \"completed\"." + "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|bool", - "description": "Used to keep track of the active affordance node ID, which is used to determine when the robot has reached its goal." + "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|str", - "description": "Used to store the ID of the door node that leads from a room to the outside world." + "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|str", - "description": "Used to represent the ID of the door node that leads from a room to the outside world." + "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": "Used to store the ID of the room node that the worker is currently entering." + "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|str", - "description": "Used to keep track of the number of vertices in the graph, which can be useful for various operations such as insertion, deletion, and querying the graph." + "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 a list of attributes that are not required for the worker's functionality, but can be useful for debugging or other purposes." + "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": "datetime|str", - "description": "Used to store the time when the worker's long-term graph was last saved. It helps track when the graph was last updated for subsequent actions." + "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 the long-term graph representation of the environment, which is updated over time as new data becomes available." + "description": "Used to store and manipulate the long-term graph data structure, which represents the agent's internal model of its environment." }, { - "name": "initialize_room_from_igraph", - "type_name": "Dict[str,Any]", - "description": "Used to initialize the room graph from an igraph graph. It takes the igraph graph as input and creates a new room graph with the same node IDs and edges as the input igraph." + "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": "update_robot_pose_in_igraph", - "type_name": "void", - "description": "Used to update the pose of a robot node in the graph by inserting a new edge with the correct \"RT\" translation and rotation." + "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": "List[Edge]", - "description": "Used to insert a new edge into the graph with the specified source and target nodes and edge type, effectively setting it as the current edge in the graph." + "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": "int|float", - "description": "Used to track the time taken by the worker to perform its tasks." + "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": "Callable[[int],str|int]", - "description": "Defined as a method that takes an integer argument and returns a string or integer value representing the result \nof some computation. It is used in various methods of the worker to perform computations on the graph." + "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": "Dict[str,Any]", - "description": "Responsible for updating a node in the graph when the worker's id matches the node's id. It takes three parameters: `id`, `type`, and `door_node`. The method updates the node's attributes based on its type and inserts an RT edge between the node and its parent if necessary." + "type_name": "None|int,str", + "description": "Used to update a node with the given id and type." }, { "name": "update_edge", - "type_name": "Union[int,str]", - "description": "Used to update an edge's type or remove it altogether based on certain conditions." + "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": 49, - "insert": 50, + "start": 52, + "insert": 53, "offset": " ", "indent": 4, "comment": null }, "item_type": "class", - "length": 628, + "length": 699, "docLength": null }, { - "id": "ad42c39d-e8e5-3180-aa4c-67d2bbbe477e", + "id": "f43929f0-c77b-5d99-7d40-e09555e1a3ab", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Initializes the worker's internal state, including its graph, node and edge variables, and timers for computing and updating the graph.", + "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": "dict", - "description": "Used to pass a mapping of proxy nodes to their corresponding real nodes in the graph." + "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 performs a check on the graph upon initialization if it's set to True, otherwise, it does not." + "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": "from SpecificWorker import SpecificWorker\nimport time\n\n# Initialize the worker with a proxy map and a startup check\nworker = SpecificWorker(proxy_map, True)\n\n# Set up the timer for the compute method\ntimer = time.time()\nworker.setPeriod(100)\nworker.startTimer()\n\nwhile True:\n # Perform the computation\n worker.compute()\n \n # Check if it's time to save the graph\n if time.time() - last_save_time > 3600:\n worker.saveGraph(\"graph.pkl\")\n", - "description": "" + "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": 50, - "insert": 51, + "start": 53, + "insert": 54, "offset": " ", "indent": 8, "comment": null }, "item_type": "constructor", - "length": 54, + "length": 65, "docLength": null }, { - "id": "61093faf-d7f7-cebb-4d49-712ed045066d", + "id": "49b3ce54-b2b8-e8ac-844b-ec91e99feeab", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Sets parameters and modifies the state of the room by removing a self-edge, storing an exit door ID, and adding attributes to doors.", + "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": "bool", - "description": "Passed to set the parameters of the room." + "type_name": "Dict[any, any]", + "description": "Expected to contain various parameters used for setting up the current room state in the game." } ], "returns": { @@ -1428,63 +1438,63 @@ }, "usage": { "language": "python", - "code": "worker = SpecificWorker(proxy_map)\n# setParams takes in a dictionary of parameters as input\nparams = {\"Period\": 10, \"agent_id\": 12}\nworker.setParams(params)\n", - "description": "\nIn this example, the user creates an instance of the SpecificWorker class and passes a dictionary of parameters to the setParams method. The function then updates the worker's internal state based on the input parameters." + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nparams = {\"agent_id\": 13, \"robot_name\": \"Shadow\"}\nresult = worker.setParams(params)", + "description": "" }, "name": "setParams", "location": { - "start": 127, - "insert": 128, + "start": 147, + "insert": 148, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 7, + "length": 9, "docLength": null }, { - "id": "cfc1e69d-8797-9087-e844-76a8a7e76242", + "id": "565d28ea-34ad-14ae-2d4a-eea4ebaac461", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Updates the robot's pose in the graph and performs various actions based on the current state of the robot.", + "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": "specific_worker = SpecificWorker()\nspecific_worker.compute()\n", + "code": "worker = SpecificWorker(proxy_map)\nwhile True:\n worker.compute()\n time.sleep(worker.Period)", "description": "" }, "name": "compute", "location": { - "start": 141, - "insert": 144, + "start": 163, + "insert": 169, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 22, + "length": 60, "docLength": null }, { - "id": "8b3d3eed-7010-bd8a-8045-f75833aa77e7", + "id": "b693ac11-a005-33bb-6c45-152db71efd4e", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Initializes a room in a graph by deleting an edge, finding the robot node and its neighbors, creating a new vertex for the root of the igraph, inserting edges to connect the robot to the room and the room to itself, and updating the robot's parent node attribute.", + "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=False)\nworker.initialize_room_from_igraph()\n", - "description": "\nThis code uses the Python function \"initialize_room_from_igraph\" to initialize the room from an igraph graph structure. The function is called on a worker object created from the SpecificWorker class, which subclasses GenericWorker." + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nworker.initialize_room_from_igraph()\n", + "description": "" }, "name": "initialize_room_from_igraph", "location": { - "start": 165, - "insert": 167, + "start": 237, + "insert": 239, "offset": " ", "indent": 8, "comment": null @@ -1494,22 +1504,22 @@ "docLength": null }, { - "id": "aab2b882-aaae-999f-b248-533b7089da3f", + "id": "b242f7eb-eb84-9880-b745-8ba8bec9e950", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Updates the robot's pose in an igraph graph based on a RT translation and rotation, taking into account the robot's current location and the surrounding rooms.", + "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": "# create an instance of SpecificWorker class\nspecific_worker = SpecificWorker(proxy_map)\n\n# call the update_robot_pose_in_igraph method to update robot pose in igraph\nspecific_worker.update_robot_pose_in_igraph()\n", - "description": "" + "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": 198, - "insert": 200, + "start": 270, + "insert": 272, "offset": " ", "indent": 8, "comment": null @@ -1519,47 +1529,47 @@ "docLength": null }, { - "id": "67757034-6f36-3890-0942-bd47b1455c70", + "id": "bced392b-adf3-4a9f-ed49-12d2abc2f0d9", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Detects when the robot enters a new room and associates doors based on their proximity to the robot's current position, updating the graph and door attributes accordingly.", + "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": "# Initialize the worker and specify the startup check parameter as True\nworker = SpecificWorker(proxy_map, startup_check=True)\n\n# Perform an initialization check for the worker\nworker.startup_check()\n\n# Set the agent ID to 13\nworker.agent_id = 13\n\n# Update the robot name to \"Shadow\"\nworker.robot_name = \"Shadow\"\n\n# Update the vertex size to 0\nworker.vertex_size = 0\n\n# Set the state of the worker to idle\nworker.state = \"idle\"\n", - "description": "" + "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": 251, - "insert": 253, + "start": 323, + "insert": 325, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 75, + "length": 86, "docLength": null }, { - "id": "51b1dbb4-c5a5-63b2-4b43-06106aa87d12", + "id": "4b50aa68-287c-e0a6-094a-b49300e80dd5", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "1) retrieves the active affordance node and its parent, 2) sets the exit door ID to the value of the parent attribute, and 3) deletes the edge connecting the current room to itself.", + "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": "import SpecificWorker\n\n# Create an instance of the SpecificWorker class\nworker = SpecificWorker(proxy_map=your_proxy_map)\n\n# Call the crossed method with no arguments and store the return value in a variable\nroom_state = worker.crossed()\n\n# Print the returned value to see if the function executed successfully\nprint(\"Room state:\", room_state)\n", - "description": "" + "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": 350, - "insert": 352, + "start": 436, + "insert": 438, "offset": " ", "indent": 8, "comment": null @@ -1569,22 +1579,22 @@ "docLength": null }, { - "id": "d55c53ae-f2c0-628c-ca4e-0ad084fa17b1", + "id": "f0eea51e-80ec-3382-9845-e7ae11931b76", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Determines and sets the ID of the room node to enter upon construction, based on conditions involving the existence of nodes labeled \"room\" and the absence of certain attributes.", + "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": "from mymodule import SpecificWorker\n\n# Creating an instance of the class and using its method initializing_room\nmyworker = SpecificWorker(proxy_map)\nmyworker.initializing_room()\n\n# The function will be executed within the context of the object,\n# allowing it to modify any attributes or methods associated with it.\n", + "code": "specific_worker = SpecificWorker(proxy_map, startup_check=True)\nspecific_worker.initializing_room()\n", "description": "" }, "name": "initializing_room", "location": { - "start": 370, - "insert": 373, + "start": 456, + "insert": 459, "offset": " ", "indent": 8, "comment": null @@ -1594,22 +1604,22 @@ "docLength": null }, { - "id": "bdadfa6f-3602-06ad-9c4d-0b5443a38eed", + "id": "b33bb58f-ff15-99bd-bf41-461d97919761", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Detects the room the robot is currently in and finds the door node leading to the next room. It then creates a new edge in the graph representing the RT pose of the robot and updates the graph with the new edge.", + "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": "# Initialize a SpecificWorker object named \"worker\"\nworker = SpecificWorker(proxy_map)\n\n# Call the known_room method with a given room name and node ID\nworker.known_room(\"room1\", 234)\n", - "description": "" + "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": 384, - "insert": 386, + "start": 470, + "insert": 472, "offset": " ", "indent": 8, "comment": null @@ -1619,83 +1629,83 @@ "docLength": null }, { - "id": "911fde00-668a-68ae-d446-d45ece69baae", + "id": "09302d92-1220-46a5-564c-abb4ed5068ba", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "1) retrieves edges leading to the exit door, 2) checks if any matching edges exist, and 3) updates node attributes and associates doors based on the matched edges.", + "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": "# create a SpecificWorker object\nworker = SpecificWorker(proxy_map)\n\n# initialize the doors in the graph\nworker.initializing_doors()\n", + "code": "worker = SpecificWorker(None, startup_check=False)\nworker.exit_door_id = 123\nworker.initializing_doors()", "description": "" }, "name": "initializing_doors", "location": { - "start": 478, - "insert": 480, + "start": 564, + "insert": 566, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 28, + "length": 26, "docLength": null }, { - "id": "17a38090-fb25-fbbd-6242-3b9b704aaad8", + "id": "860d158e-bcd7-bbb3-ff4f-c34b26f5454f", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Connects two nodes representing doors in the long-term graph, adding an edge between them and updating their attributes to reflect the connection.", + "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": "str", - "description": "A list containing the name of the first door to be associated with another door." + "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": "str", - "description": "A string representation of the second door to be associated with the long-term graph." + "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": "# Initialize the SpecificWorker class object\nworker = SpecificWorker(proxy_map)\n\n# Define two door objects with names and room identifiers\ndoor1 = {\"name\": \"Front Door\", \"room_id\": 2}\ndoor2 = {\"name\": \"Back Door\", \"room_id\": 4}\n\n# Call the associate_doors method on the worker object\nworker.associate_doors(door1, door2)\n", + "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": 517, - "insert": 519, + "start": 601, + "insert": 603, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 16, + "length": 20, "docLength": null }, { - "id": "cc21732a-a5cc-7d90-2b4b-65d37df3c117", + "id": "7d9e23ea-fe30-b6b7-f846-d999e5057864", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Saves the graph representation of a room node and its exit door id to a file.", + "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": "from worker import SpecificWorker\n\n# Initialize the worker\nworker = SpecificWorker(proxy_map, startup_check=False)\n\n# Create a new room node\nroom_node = worker.g.vs.find(name=\"New Room\")\nif not room_node:\n room_node = worker.g.add_vertex(name=\"New Room\")\n\n# Store the graph in pickle format\nwith open(\"graph.pkl\", \"wb\") as f:\n pickle.dump(worker.long_term_graph.g, f)\n", + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nworker.store_graph()", "description": "" }, "name": "store_graph", "location": { - "start": 536, - "insert": 537, + "start": 624, + "insert": 625, "offset": " ", "indent": 8, "comment": null @@ -1705,22 +1715,22 @@ "docLength": null }, { - "id": "f099c4de-b784-939e-6846-a148b2aeec31", + "id": "242f4fee-7403-e2be-6f4d-a278ec8a96c9", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Removes edges from the long-term graph based on certain conditions, including room numbers and types of edges.", + "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": "specific_worker = SpecificWorker(proxy_map, startup_check=True)\nspecific_worker.removing()\n", - "description": "\nThis code creates a new instance of the `SpecificWorker` class and then calls the `removing` method on it. The `removing` function is defined in the `SpecificWorker` class, and it removes certain elements from the graph using the `delete_node` method. The graph is an attribute of the `SpecificWorker` class, and it represents a long-term spatial memory for a robot named Shadow. The `startup_check` argument passed to the `SpecificWorker` constructor is set to `True`, indicating that the startup check should be performed before the removal process can begin." + "code": "worker = SpecificWorker(proxy_map, startup_check=True)\nworker.removing()", + "description": "" }, "name": "removing", "location": { - "start": 550, - "insert": 552, + "start": 638, + "insert": 640, "offset": " ", "indent": 8, "comment": null @@ -1730,28 +1740,28 @@ "docLength": null }, { - "id": "18553e7e-c150-41ac-6742-b52ca312d465", + "id": "77fff487-b57f-4fa4-1d44-3789f5a13b53", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Traverses a graph and performs actions based on the type of edges it encounters. It starts at a designated node, then visits the children nodes of that node according to the RT edge type, and repeats this process for each child node until no more actions are left to perform.", + "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": "Represented by the igraph vertex ID of the current node being traversed." + "description": "Expected to be an identifier for a node within the graph data structure (`self.g`)." } ], "returns": null, "usage": { "language": "python", - "code": "specific_worker = SpecificWorker(proxy_map, startup_check=False)\nnode_id = \"a_node_id\"\nspecific_worker.traverse_graph(node_id)\n", - "description": "\nThis example shows how the traverse_graph method of the SpecificWorker class can be used to start traversing a graph from a specific node. The node ID is passed as an argument to the function, and the function then performs the necessary steps to traverse the graph starting at that node." + "code": "worker = SpecificWorker(proxy_map)\nnode_id = 5\nworker.traverse_graph(node_id)", + "description": "" }, "name": "traverse_graph", "location": { - "start": 576, - "insert": 578, + "start": 664, + "insert": 666, "offset": " ", "indent": 8, "comment": null @@ -1761,28 +1771,28 @@ "docLength": null }, { - "id": "8ac6a6a7-f160-089e-bb4c-9b09f663081c", + "id": "edceaa7c-333f-dd99-9e44-b9bdbae031be", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Traverses the graph, identifying vertices with higher level than the current vertex and adding them to the DSR graph along with their connections.", + "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.Vertex", - "description": "Representing a vertex in the graph." + "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": "specific_worker = SpecificWorker(proxy_map)\nspecific_worker.traverse_igraph(specific_worker.g.vs[0])\n", - "description": "\nThis code creates a new object of type SpecificWorker and then calls the traverse_igraph method, passing in the first vertex of the graph as an argument. This vertex is used to start the traversal of the graph." + "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": 586, - "insert": 587, + "start": 674, + "insert": 675, "offset": " ", "indent": 8, "comment": null @@ -1792,28 +1802,28 @@ "docLength": null }, { - "id": "8d4576e6-5bd3-6d9d-6548-08acb2abc696", + "id": "a71b8f95-8dea-f190-3143-5b6d1f0a1dfd", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Adds a new vertex to an existing graph, updating the graph's vertices with provided attributes and matching other vertices based on specified conditions.", + "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": "igraph.Vertex", - "description": "Used to represent a single vertex in the graph, including its name, ID, and attributes." + "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": "worker = SpecificWorker() # Initiate the class\nnode = Node(name=\"Door\", id=1, type=\"door\") # Create a node with name and ID\nworker.insert_igraph_vertex(node) # Pass the node to the function\n", - "description": "\nThe example shows how a node with name \"Door\" and ID \"1\" is created and then passed to the insert_igraph_vertex() function, which will add it as a vertex in the graph. The function also takes care of any other attributes that may be present in the node object, such as parent or timestamp creation." + "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": 599, - "insert": 600, + "start": 687, + "insert": 688, "offset": " ", "indent": 8, "comment": null @@ -1823,33 +1833,33 @@ "docLength": null }, { - "id": "705003db-9448-d6bc-2145-c9b70fa2a402", + "id": "021e56e0-815f-d9a6-5343-d8f2a6d2e7bd", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Modifies an existing node within a graph, adding new attributes and linking it to its parent node through an attribute.", + "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 specify the name of the parent node to insert the new vertex into." + "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": "Node | dict", - "description": "Passed in from caller, it contains attributes that define the vertex to be inserted into graph." + "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)\nspecific_worker.insert_dsr_vertex(\"parent_name\", {\"type\": \"node_type\", \"name\": \"node_name\"})\n", - "description": "\nThis example inserts a node with the name \"node_name\" into the graph with the type \"node_type\" and links it to the parent node named \"parent_name\"." + "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": 634, - "insert": 636, + "start": 722, + "insert": 724, "offset": " ", "indent": 8, "comment": null @@ -1859,64 +1869,64 @@ "docLength": null }, { - "id": "57a732e1-901a-ef80-944c-23f4f0907fda", + "id": "1c8acec2-134d-5faf-3c43-92947a256819", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Adds an edge to a long-term graph based on a received edge attribute. It checks if the destination node is the robot, and if so, uses the correct translation and rotation attributes. Otherwise, it uses the provided translation and rotation attributes.", + "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": "GraphEdge", - "description": "Passed as a whole object containing information about an edge in the graph, including its origin and destination nodes, as well as attribute values such as translation and rotation." + "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\", \"Destination\")\nedge.attrs[\"rt_translation\"] = [10, 20, 30]\nedge.attrs[\"rt_rotation_euler_xyz\"] = [45, 90, 180]\nworker.insert_igraph_edge(edge)\n", - "description": "\nIn this example, a user creates an instance of the SpecificWorker class and then adds an edge to the graph by calling the insert_igraph_edge method with the edge object as an argument. The edge object contains information about the origin and destination nodes of the edge, such as their names or IDs. The method then uses this information to add a new edge to the long-term spatial memory graph with the given translation and rotation attributes." + "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": 648, - "insert": 649, + "start": 736, + "insert": 737, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 12, + "length": 15, "docLength": null }, { - "id": "3f1ca468-3163-3ba8-e54f-a2fb9ce810fa", + "id": "30527036-60e3-8886-a946-2f1b41d32ef6", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Creates a new edge in the long-term graph based on data from the short-term graph. It assigns attributes to the new edge, including a translation and rotation value for RT tracking.", + "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": "GraphNode | NoneType", - "description": "Used to specify the starting node of the edge to be inserted. If it is None, it means the starting node is unknown or unspecified." + "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": "Node | str", - "description": "Used to specify the destination node or edge id for the edge insertion operation." + "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": "worker = SpecificWorker(proxy_map)\n# Define the nodes and edges in the graph\nnode1 = Node(\"Node 1\", agent_id=worker.agent_id)\nnode2 = Node(\"Node 2\", agent_id=worker.agent_id)\nedge = Edge(node1, node2, \"RT\", worker.agent_id)\n# Call the function to insert the edge into the graph\nworker.insert_dsr_edge(node1, node2)\n", + "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": 665, - "insert": 668, + "start": 757, + "insert": 760, "offset": " ", "indent": 8, "comment": null @@ -1926,31 +1936,31 @@ "docLength": null }, { - "id": "04491164-d053-7abe-e94a-0772c85a56b6", + "id": "b71963ca-d296-d386-3542-ca6aca921929", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Determines the room number associated with a given node ID by retrieving the attribute \"room_id\" from the node and returning its value if found, or -1 otherwise.", + "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", - "description": "Passed as an argument to the function, indicating the id of the element whose room number needs to be checked." + "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": "int", - "description": "The room ID associated with a given node ID if the node has an attribute \"room_id\" and its value is not None, or -1 otherwise." + "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(proxy_map)\nnode_id = 1234567890\nroom_id = worker.check_element_room_number(node_id)\nif room_id == -1:\n print(\"Node is not in a room\")\nelse:\n print(\"Node is in room\", room_id)\n", - "description": "" + "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": 695, - "insert": 696, + "start": 787, + "insert": 788, "offset": " ", "indent": 8, "comment": null @@ -1960,31 +1970,31 @@ "docLength": null }, { - "id": "f912f9d3-d6ed-b482-ee40-a7c750e382d7", + "id": "71c5bfb8-3835-7788-9042-be2ab507cbc5", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "1) retrieves the element level of a node with the given ID, and 2) handles missing or non-existent element levels by printing an error message and returning -1.", + "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": "int", - "description": "Representing an element node identifier, which identifies a specific element in the graph." + "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", - "description": "Element level if found, or -1 otherwise." + "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": "# Initialize SpecificWorker class instance\nworker = SpecificWorker(proxy_map)\n\n# Get node_id of a specific element in the graph\nnode_id = 123456789\n\n# Call check_element_level function to retrieve level attribute value from node\nelement_level = worker.check_element_level(node_id)\n\n# Print retrieved level attribute value for the specified element\nprint(\"Element level:\", element_level)\n", + "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": 704, - "insert": 705, + "start": 796, + "insert": 797, "offset": " ", "indent": 8, "comment": null @@ -1994,28 +2004,28 @@ "docLength": null }, { - "id": "1141fbc6-886b-ab8c-324d-576e1b3fb114", + "id": "8d17504d-3d97-6684-5142-8bb1f661e2e4", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Retrieves and processes information about a room node in a graph, including its ID, old RT edges, translation attribute, and doors. It then draws the room polygon and doors on an image for saving.", + "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": "str | int", - "description": "Used to identify the room for which the picture will be generated." + "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": "specific_worker = SpecificWorker(proxy_map)\n# Startup check and initialize the robot's name, id, and last robot pose.\nspecific_worker.startup_check()\n\n# Generate the room picture using the specific worker instance and the desired room node ID.\nroom_node_id = 123456 # Replace with actual node ID\nspecific_worker.generate_room_picture(room_node_id)\n", + "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": 747, - "insert": 749, + "start": 839, + "insert": 841, "offset": " ", "indent": 8, "comment": null @@ -2025,28 +2035,28 @@ "docLength": null }, { - "id": "abb52b39-2b77-1e95-5943-2ff31d0cfa5b", + "id": "0e858f9e-90d2-f5ac-9c4d-3bca8d1e9ec8", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Inserts or assigns an edge in the graph with the current room ID as the tail and the same ID as the head, indicating that the agent is currently located in that room.", + "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": "str", - "description": "Used to represent the id of the current room in which the agent is located." + "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)\nworker.insert_current_edge(room_id)\n", + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\nroom_id = \"12345\"\nworker.insert_current_edge(room_id)", "description": "" }, "name": "insert_current_edge", "location": { - "start": 782, - "insert": 784, + "start": 874, + "insert": 876, "offset": " ", "indent": 8, "comment": null @@ -2056,74 +2066,99 @@ "docLength": null }, { - "id": "c580b131-a2f7-48b2-d347-fd8315f9d58a", + "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": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Updates the node attributes based on user input and inserts new edges into the graph according to predefined rules.", + "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 to identify the node to be updated." + "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 identify the type of node being updated (door or room)." + "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": "# Call the function update_node() with parameters\nworker.update_node(1, \"door\")\n", - "description": "\nIn this code snippet, we call the `update_node()` function in the `SpecificWorker` class by passing two arguments: 1 and \"door\". The first argument is the id of the node to be updated, and the second argument is the type of the node, which is door in this case. This call will update the node with the id 1 with the new type information.\n\nWe can also use `update_node()` to update other types of nodes, such as rooms or affordances, by passing different arguments for the type parameter. For example:\n" + "code": "worker = SpecificWorker(proxy_map, startup_check=False)\ndoor_id = 42\nworker.update_node(door_id, \"door\")", + "description": "" }, "name": "update_node", "location": { - "start": 796, - "insert": 797, + "start": 902, + "insert": 903, "offset": " ", "indent": 8, "comment": null }, "item_type": "method", - "length": 41, + "length": 36, "docLength": null }, { - "id": "1b073fde-9b9f-83a1-3541-533c6e7e206e", + "id": "2f5f1ef6-a378-a8ba-2246-9cb42852f320", "ancestors": [ - "68a48220-ebf4-7cb1-6d46-ec8ae0b47c1b" + "895186b9-f902-39ab-ec46-de7001893323" ], - "description": "Updates an edge in the graph based on its type, source node, and target node.", + "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": "Representing the first room number of an edge to be updated with new information." + "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 identify the target node in the graph for updating the edge." + "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": "Used to indicate whether the edge is a robot or a room node." + "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, startup_check=False)\n# Update edge in graph\nworker.update_edge(fr=1, to=2, type=\"RT\")\n", - "description": "\nThis updates the edge with fr as the starting node and to as the ending node with the type of \"RT\"." + "code": "worker = SpecificWorker(proxy_map)\nworker.update_edge(42, 13, \"RT\")", + "description": "" }, "name": "update_edge", "location": { - "start": 851, - "insert": 855, + "start": 957, + "insert": 961, "offset": " ", "indent": 8, "comment": null diff --git a/.komment/komment.json b/.komment/komment.json index d789db51..ee4ecedf 100644 --- a/.komment/komment.json +++ b/.komment/komment.json @@ -1,7 +1,7 @@ { "meta": { "version": "1", - "updated_at": "2024-07-19T07:43:08.491Z", + "updated_at": "2024-07-31T13:11:04.283Z", "created_at": "2024-07-03T15:48:27.349Z", "pipelines": [ "50c96b7c-ad3b-4b93-ae19-7f6fbd5637f7", @@ -12,7 +12,8 @@ "395b32ba-01b1-45bb-84e0-96a1b0abb44e", "fb0c09b8-650c-42fb-a814-0c07d121ec67", "3216d745-95dc-419d-8524-71b95ba17b45", - "bdcf9dc7-844d-44d2-af0f-6fcd7dfa4a7c" + "bdcf9dc7-844d-44d2-af0f-6fcd7dfa4a7c", + "2a176ad9-6055-453c-8640-de6390f8f6fd" ] }, "lookup": [ diff --git a/agents/g2o_agent/src/specificworker.py b/agents/g2o_agent/src/specificworker.py index 7b23b755..21e87720 100644 --- a/agents/g2o_agent/src/specificworker.py +++ b/agents/g2o_agent/src/specificworker.py @@ -49,126 +49,128 @@ class SpecificWorker(GenericWorker): """ - Manages a graph-based representation of an environment and its objects, providing - functions for updating node and edge attributes, deleting nodes and edges, and - handling RT messages. It also maintains a set of rooms and their initial positions. + 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 (float|int): Set to `200` by default, indicating the time interval - (in milliseconds) between successive calls to the `update` method. It - determines how frequently the worker updates its state. - agent_id (int|str): Used to identify the worker's agent in the simulation - environment. - g (Graph|NetworkXGraph): Used for managing graphs in the worker's update - and delete operations. - startup_check (QTimersingleShot200,QApplicationinstancequit): Used to check - if the application should quit after a certain period of time. - rt_api (str|int): Used to store the last RT (Real-time) edge set id that - was received by the worker, so it can check if a new RT edge set has - been received since the last time it was checked. - inner_api (Callable[[],None]): Used to define a custom API for the worker - to interact with its internal state and update its graph. - odometry_node_id (int): Used to store the ID of the node representing the - shadow robot in the graph, which is used for updating the robot's odometry. - odometry_queue (List[Tuple[float,float,float,int]]): Used to store the - odometry data points received from the ROS node. It is updated every - 200 milliseconds. - last_odometry (float|List[float]): Used to store the last known odometry - values (position, orientation, and advance speed) for the robot. - g2o (None|Graph2O): Used to store the graph data in Graph2O format for the - robot's environment. - odometry_noise_std_dev (float|int): Used to control the noise level in - robot odometry readings. - odometry_noise_angle_std_dev (float|int): 0.1 by default, representing the - standard deviation of the angle noise added to the robot's odometry - measurements during simulation. - measurement_noise_std_dev (float|double): Used to represent the standard - deviation of measurement noise in the worker's measurements. It is - used to simulate random fluctuations in the measurements during training. - last_room_id (int|str): Used to store the last room ID seen by the worker - before changing rooms, which allows the worker to track the current - room ID. - actual_room_id (str|int): Used to keep track of the current room ID that - the worker is in, during its execution - elapsed (float|int): Used to store the elapsed time since the worker's - last call to the `update` method, which can be used to control the - worker's execution rate. - room_initialized (bool): Used to track whether a specific room has been - initialized for RT mapping. It is set to False when the room is first - encountered, and True when it has been successfully mapped with the - robot's pose. - iterations (int|float): Used to keep track of the number of iterations of - the worker's tasks that have been performed. - hide (bool): Used to hide or show the worker's updates during simulation. - init_graph (bool): Set to True when the worker initializes its graph and - False otherwise, indicating whether the graph has been initialized or - not. - current_edge_set (bool): Used to keep track of whether the current edge - set has been updated recently, indicating when a new RT translation - or rotation should be computed. It is set to True after each edge set - update and reset to False after a certain time interval (defined as - `rt_time_min` in the code) has passed without any updates, to avoid - computing unnecessary RT translations or rotations. - first_rt_set (bool): Set to True when the RT translation and rotation are - first detected, False otherwise. It indicates whether the RT set has - been detected for a given edge. - translation_to_set (float|List[float]): Used to store the translation of - the robot to set. It is updated when the RT transmission is received - and its value represents the distance from the origin of the robot's - frame to the origin of the set frame in the global coordinate system. - rotation_to_set (float|List[float]): Used to store the Euler angles - representing the rotation from the current room to the target set. - room_polygon (List[float]): Used to store the coordinates of a polygon - representing the boundary of a room in the environment, which is used - for collision detection and avoidance. - security_polygon (Shape|str): Used to store a polygon that defines the - security area around the robot, which is used for collision detection - and obstacle avoidance. - initialize_g2o_graph (void): Used to initialize a Graph2O graph, which is - a data structure representing a robot's environment, and perform other - operations such as adding nodes and edges, setting attributes, and - updating node positions. - rt_set_last_time (float|int): Used to track the time since the last RT set - was created by the worker. It is used to determine when to set a new - RT translation and rotation value. - rt_time_min (float|int): Defined as the minimum time interval between - consecutive RT sets. Its purpose is to ensure that the robot's motion - is smooth and does not oscillate excessively during navigation. - last_update_with_corners (int|bool): Used to keep track of the last time - corners were updated. When the corner of a room was updated, it's set - to True, otherwise it's set to False. - timer (QTimersingleShot200,QApplicationinstancequit): Used to schedule a - call to the `QApplication.instance().quit()` function after 200 - milliseconds. This allows the worker to run indefinitely until the - user presses the "Stop" button. - compute (Callable[[],float]): Used to compute the next node id to update - based on the current node id and the edge id. It takes no arguments - and returns a floating-point value representing the next node id to update. - update_node_att (List[str]): Used to update the attributes of a node in - the graph based on certain conditions, such as when the node's id - matches the `odometry_node_id` constant. - update_edge (Callable[[int,int,str],None]): Called when an edge's type - changes to "RT". It sets the `translation_to_set`, `rotation_to_set`, - and `rt_set_last_time` attributes based on the new RT translation and - rotation. - update_edge_att (List[str]): Used to update the attributes of an edge based - on a specific condition. It takes three parameters: `fr`, `to`, and - `type`, which are the ID of the edge being updated, its destination - ID, and the type of update respectively. + 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 components of the worker, including the graph, odometry - queue, and timer. It also sets up signal connections for updates on nodes - and edges. + 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 (Dict[str, Any]): Used to store map-related information, - such as the graph's nodes and edges, which are required for the - worker's functionality. - startup_check (bool): Used to check if the graph has been properly - initialized before proceeding with the worker's tasks. + 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) @@ -245,12 +247,10 @@ def setParams(self, params): @QtCore.Slot() def compute(self): """ - Performs the following tasks: - 1/ Checks if enough time has passed since the last frame was computed. - 2/ Updates the robot's position and orientation using odometry data. - 3/ Generates a new RT edge for the robot based on its current position and - orientation. - 4/ Adds the new RT edge to the graph. + 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: @@ -385,13 +385,13 @@ def initialize_g2o_graph(self): # print("Initializing g2o graph") # get robot pose in room """ - 1) retrieves nodes and edges from the ROS topic, 2) validates node and - edge presence, 3) adds nominal corners and fixed poses to the G2O graph - for robot and door nodes, and 4) initializes last odometry timestamp. + 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: 1 if the initialization of the G2O graph was successful, and 0 - otherwise. + bool|None: True if successful and initializes a G2O graph, otherwise + it returns None or False indicating failure. """ self.g2o.clear_graph() @@ -445,7 +445,7 @@ def initialize_g2o_graph(self): # 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 = 500 + d = 350 self.security_polygon = QPolygonF() if self.room_polygon is not None: landmark_information = np.array([[0.05, 0.0], @@ -598,16 +598,18 @@ def initialize_g2o_graph(self): def get_displacement(self, odometry): """ - Calculates the displacement of a robot based on its odometry data, computing - the lateral displacement, forward displacement, and angular displacement. + 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]): A sequence of 3-element tuples - representing the robot's position, velocity, and timestamp. + odometry (Tuple[float, float, float, float]): Assumed to represent the + current odometry data. Returns: - Tuple[float,float,float]: 3 components of displacement (lateral, avance - and angular) calculated based on the odometry data. + Tuple[float,float,float]: A tuple containing three float values + representing the lateral displacement, advance displacement, and angular + displacement respectively. """ desplazamiento_avance = 0 @@ -630,15 +632,18 @@ def get_displacement(self, odometry): def get_covariance_matrix(self, vertex): """ - Computes the covariance matrix of a set of vertices in a graph using the - G2O optimizer and returns the result. + 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 (G2O.HessianIndex): Used to represent a vertex in the graph. + vertex (object): Expected to have attributes such as `hessian_index`. Returns: - Tuple[bool,npndarray]: A result of computing marginals and a covariance - matrix. + 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())] @@ -655,11 +660,13 @@ def get_covariance_matrix(self, vertex): def visualize_g2o_realtime(self, optimizer): """ - Loads G2O files, visualizes their positions and edges in 3D, and updates - the display in real-time using matplotlib. + Visualizes the estimated positions of vertices and edges from a G2O + optimization process in real-time using matplotlib's 3D plotting functionality. Args: - optimizer (object): Used to load a G2O file for visualization. + 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() @@ -708,15 +715,18 @@ def update_node_att(self, id: int, attribute_names: [str]): # print("INIT GRAPH") """ - Updates the attributes of a node in a graph, specifically the odometry - node, and appends a new entry to the odometry queue based on the current - robot position and speed. + 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): Used to identify the node for which attributes are being - updated, specifically the odometry node. - attribute_names ([str]): An array of strings that represents the names - of attributes to be updated on a node in the graph. + 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: @@ -737,25 +747,28 @@ def update_node(self, id: int, type: str): # self.room_initialized = True if self.initialize_g2o_graph() else False # self.init_graph = True """ - Updates an individual node's type, specifically "corner". If the node - belongs to a room, it initializes the graph's room structure if necessary. + 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): Used to identify the node being updated. - type (str): Used to identify the node's type, which can be either - "corner" or "room". + 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 from a graph data structure represented in the method - signature, by setting the `room_initialized` attribute to `False`. + 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): Intended to represent the unique identifier of the node to - be deleted. + id (int): Expected to receive an integer value representing the ID of + a node to be deleted. """ pass @@ -768,15 +781,19 @@ def delete_node(self, id: int): def update_edge(self, fr: int, to: int, type: str): """ - Updates the room ID and sets the current edge set based on node type and - RT edge information. + 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): Representing the from node ID in the graph. - to (int): Used to represent the ID of the node that follows the edge - being updated. - type (str): Used to specify the edge's type, which can be either - "current" or "RT". + 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": @@ -808,15 +825,15 @@ def update_edge_att(self, fr: int, to: int, type: str, attribute_names: [str]): def delete_edge(self, fr: int, to: int, type: str): """ - Deletes an edge from a graph, specified by its index (fr), type (to), and - the worker instance. + 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): 1st in the function signature, indicating that it should be - the first argument passed to the function when calling it. - to (int): Representing the second vertex index of an edge to be deleted. - type (str): Used to specify the edge type to be deleted, which can be - either "weighted" or "unweighted". + 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 diff --git a/agents/long_term_spatial_memory_agent/src/specificworker.py b/agents/long_term_spatial_memory_agent/src/specificworker.py index 132e747b..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 @@ -48,92 +51,102 @@ class SpecificWorker(GenericWorker): """ - Manages a specific node and edge attributes and performs various operations - on them, such as updating, inserting, deleting, and attributing. It also keeps - track of the current state of the robot and updates the graph accordingly. + 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 (Union[int,str]): Used to store the period of time during which the - worker can perform its task, such as a day or a week. - agent_id (int|str): Used to identify the specific worker instance, allowing - the agent to perform tasks and maintain a unique identity during its - lifetime. - g (igraphGraph): Used to represent the long-term graph of the environment, - which is updated periodically by the worker. - startup_check (bool|str): Used to check if the worker has been started or - not, with possible values "yes", "no", or False. - rt_api (str|int): Used for storing the RT (Real-Time) API ID which represents - the current task being executed by the worker. - inner_api (Dict[str,Any]): Used to store the inner API of the worker, which - is a Python function that performs actions related to the worker's task. - robot_name (str|int): Used to store the name of the robot that will perform - a specific task. - robot_id (int): Used to identify the robot node in the graph, which is the - starting point for the worker's movement. - last_robot_pose (Tuple[float,float,float]): Stored as the last known pose - (position, orientation, and scale) of the robot before any actions - were taken in the environment. - robot_exit_pose (str|int): A reference to the pose of the robot when it - exits a room. It is used to determine when the robot has reached its - goal in a task. - state (str|int): Used to keep track of the current state of the worker, - which can be either "idle", "crossed", or "completed". - affordance_node_active_id (int|bool): Used to keep track of the active - affordance node ID, which is used to determine when the robot has - reached its goal. - exit_door_id (int|str): Used to store the ID of the door node that leads - from a room to the outside world. - room_exit_door_id (int|str): Used to represent the ID of the door node - that leads from a room to the outside world. - enter_room_node_id (int): Used to store the ID of the room node that the - worker is currently entering. - vertex_size (int|str): Used to keep track of the number of vertices in the - graph, which can be useful for various operations such as insertion, - deletion, and querying the graph. - not_required_attrs (List[str]): Used to store a list of attributes that - are not required for the worker's functionality, but can be useful for - debugging or other purposes. - last_save_time (datetime|str): Used to store the time when the worker's - long-term graph was last saved. It helps track when the graph was last - updated for subsequent actions. - long_term_graph (igraphGraph): Used to store the long-term graph representation - of the environment, which is updated over time as new data becomes available. - initialize_room_from_igraph (Dict[str,Any]): Used to initialize the room - graph from an igraph graph. It takes the igraph graph as input and - creates a new room graph with the same node IDs and edges as the input - igraph. - update_robot_pose_in_igraph (void): Used to update the pose of a robot - node in the graph by inserting a new edge with the correct "RT" - translation and rotation. - insert_current_edge (List[Edge]): Used to insert a new edge into the graph - with the specified source and target nodes and edge type, effectively - setting it as the current edge in the graph. - timer (int|float): Used to track the time taken by the worker to perform - its tasks. - compute (Callable[[int],str|int]): Defined as a method that takes an integer - argument and returns a string or integer value representing the result - of some computation. It is used in various methods of the worker to - perform computations on the graph. - update_node (Dict[str,Any]): Responsible for updating a node in the graph - when the worker's id matches the node's id. It takes three parameters: - `id`, `type`, and `door_node`. The method updates the node's attributes - based on its type and inserts an RT edge between the node and its - parent if necessary. - update_edge (Union[int,str]): Used to update an edge's type or remove it - altogether based on certain conditions. + 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 the worker's internal state, including its graph, node and - edge variables, and timers for computing and updating the graph. + 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 (dict): Used to pass a mapping of proxy nodes to their - corresponding real nodes in the graph. - startup_check (bool): Set to False by default. It performs a check on - the graph upon initialization if it's set to True, otherwise, it - does not. + 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) @@ -149,6 +162,8 @@ def __init__(self, proxy_map, startup_check=False): 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 @@ -175,16 +190,29 @@ def __init__(self, proxy_map, startup_check=False): # Global map variables self.long_term_graph = LongTermGraph("graph.pkl") # 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.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 @@ -195,6 +223,8 @@ 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) @@ -214,11 +244,13 @@ def __del__(self): def setParams(self, params): """ - Sets parameters and modifies the state of the room by removing a self-edge, - storing an exit door ID, and adding attributes to doors. + 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 (bool): Passed to set the parameters of the room. + params (Dict[any, any]): Expected to contain various parameters used + for setting up the current room state in the game. Returns: bool: True. @@ -226,6 +258,8 @@ def setParams(self, params): """ 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, @@ -240,14 +274,65 @@ def setParams(self, params): @QtCore.Slot() def compute(self): # Exectue function self.update_robot_pose_in_igraph each second + # if time.time() - self.last_save_time > 1: + # self.update_robot_pose_in_igraph() + """ - Updates the robot's pose in the graph and performs various actions based - on the current state of the robot. + 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 time.time() - self.last_save_time > 1: - self.update_robot_pose_in_igraph() + 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() @@ -269,10 +354,9 @@ def compute(self): def initialize_room_from_igraph(self): # Remove RT edge between "root" and "Shadow" """ - Initializes a room in a graph by deleting an edge, finding the robot node - and its neighbors, creating a new vertex for the root of the igraph, - inserting edges to connect the robot to the room and the room to itself, - and updating the robot's parent node attribute. + 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") @@ -309,9 +393,10 @@ def initialize_room_from_igraph(self): def update_robot_pose_in_igraph(self): # # Check if graph exists """ - Updates the robot's pose in an igraph graph based on a RT translation and - rotation, taking into account the robot's current location and the surrounding - rooms. + 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: @@ -368,9 +453,9 @@ def update_robot_pose_in_igraph(self): 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 """ - Detects when the robot enters a new room and associates doors based on - their proximity to the robot's current position, updating the graph and - door attributes accordingly. + 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] @@ -388,6 +473,20 @@ def idle(self): exit_room_node = self.g.get_node(self.room_exit_door_id) # Store DSR graph in igraph self.store_graph() + # 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(False) # Compute metric map and draw it @@ -473,9 +572,8 @@ def idle(self): def crossed(self): # Get parent node of affordance node """ - 1) retrieves the active affordance node and its parent, 2) sets the exit - door ID to the value of the parent attribute, and 3) deletes the edge - connecting the current room to itself. + 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) @@ -500,9 +598,10 @@ def initializing_room(self): # Get room nodes """ - Determines and sets the ID of the room node to enter upon construction, - based on conditions involving the existence of nodes labeled "room" and - the absence of certain attributes. + 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] @@ -519,9 +618,9 @@ def initializing_room(self): def known_room(self): # Get other side door name attribute """ - Detects the room the robot is currently in and finds the door node leading - to the next room. It then creates a new edge in the graph representing the - RT pose of the robot and updates the graph with the new edge. + 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) @@ -619,9 +718,9 @@ def known_room(self): def initializing_doors(self): # Check if node called "room_entry" of type room exists """ - 1) retrieves edges leading to the exit door, 2) checks if any matching - edges exist, and 3) updates node attributes and associates doors based on - the matched edges. + 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] @@ -655,34 +754,38 @@ 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 """ - Connects two nodes representing doors in the long-term graph, adding an - edge between them and updating their attributes to reflect the connection. + 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 (str): A list containing the name of the first door to be - associated with another door. - door_2 (str): A string representation of the second door to be associated - with the long-term graph. + 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.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.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.long_term_graph.g.add_edge(door_1_node, door_2_node) door_1_node["other_side_door_name"] = door_2[0] @@ -693,7 +796,9 @@ def associate_doors(self, door_1, door_2): def store_graph(self): """ - Saves the graph representation of a room node and its exit door id to a file. + 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) @@ -712,8 +817,10 @@ def store_graph(self): def removing(self): # # Get last number in the name of the room """ - Removes edges from the long-term graph based on certain conditions, including - room numbers and types of edges. + 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 @@ -743,14 +850,13 @@ def removing(self): def traverse_graph(self, node_id): # Mark the current node as visited and print it """ - Traverses a graph and performs actions based on the type of edges it - encounters. It starts at a designated node, then visits the children nodes - of that node according to the RT edge type, and repeats this process for - each child node until no more actions are left to perform. + 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): Represented by the igraph vertex ID of the current node - being traversed. + 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) @@ -763,11 +869,15 @@ def traverse_graph(self, node_id): def traverse_igraph(self, node): """ - Traverses the graph, identifying vertices with higher level than the current - vertex and adding them to the DSR graph along with their connections. + 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.Vertex): Representing a vertex in the graph. + 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) @@ -784,12 +894,15 @@ def traverse_igraph(self, node): def insert_igraph_vertex(self, node): """ - Adds a new vertex to an existing graph, updating the graph's vertices with - provided attributes and matching other vertices based on specified conditions. + 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 (igraph.Vertex): Used to represent a single vertex in the graph, - including its name, ID, and attributes. + 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) @@ -829,14 +942,17 @@ def insert_igraph_vertex(self, node): def insert_dsr_vertex(self, parent_name, node): # print("Inserting vertex", node["name"], node["type"]) """ - Modifies an existing node within a graph, adding new attributes and linking - it to its parent node through an attribute. + 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 specify the name of the parent node to - insert the new vertex into. - node (Node | dict): Passed in from caller, it contains attributes that - define the vertex to be inserted into graph. + 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"]) @@ -853,15 +969,14 @@ def insert_dsr_vertex(self, parent_name, node): id_result = self.g.insert_node(new_node) def insert_igraph_edge(self, edge): """ - Adds an edge to a long-term graph based on a received edge attribute. It - checks if the destination node is the robot, and if so, uses the correct - translation and rotation attributes. Otherwise, it uses the provided - translation and rotation attributes. + 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 (GraphEdge): Passed as a whole object containing information about - an edge in the graph, including its origin and destination nodes, - as well as attribute values such as translation and rotation. + 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) @@ -872,7 +987,11 @@ def insert_igraph_edge(self, edge): destination_node = self.long_term_graph.g.vs.find(name=destination_node_dsr.name) # Add the edge to the graph if destination_node_dsr.name != self.robot_name: - self.long_term_graph.g.add_edge(origin_node, destination_node, rt=edge.attrs["rt_translation"].value, rotation=edge.attrs["rt_rotation_euler_xyz"].value) + # 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"]) @@ -884,16 +1003,17 @@ def insert_dsr_edge(self, org, dest): # print("ORG::", org) # self.insert_dsr_vertex(dest) """ - Creates a new edge in the long-term graph based on data from the short-term - graph. It assigns attributes to the new edge, including a translation and - rotation value for RT tracking. + 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 (GraphNode | NoneType): Used to specify the starting node of the - edge to be inserted. If it is None, it means the starting node is - unknown or unspecified. - dest (Node | str): Used to specify the destination node or edge id for - the edge insertion operation. + 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: @@ -925,17 +1045,17 @@ def insert_dsr_edge(self, org, dest): def check_element_room_number(self, node_id): """ - Determines the room number associated with a given node ID by retrieving - the attribute "room_id" from the node and returning its value if found, - or -1 otherwise. + 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): Passed as an argument to the function, indicating the - id of the element whose room number needs to be checked. + node_id (int | str): Required for this function. It represents an + identifier that uniquely identifies a node within a graph data structure. Returns: - int: The room ID associated with a given node ID if the node has an - attribute "room_id" and its value is not None, or -1 otherwise. + 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) @@ -948,16 +1068,19 @@ def check_element_room_number(self, node_id): def check_element_level(self, node_id): """ - 1) retrieves the element level of a node with the given ID, and 2) handles - missing or non-existent element levels by printing an error message and - returning -1. + 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 (int): Representing an element node identifier, which identifies - a specific element in the graph. + 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: Element level if found, or -1 otherwise. + 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) @@ -1005,13 +1128,13 @@ def check_element_level(self, node_id): def generate_room_picture(self, room_node_id): """ - Retrieves and processes information about a room node in a graph, including - its ID, old RT edges, translation attribute, and doors. It then draws the - room polygon and doors on an image for saving. + 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 (str | int): Used to identify the room for which the - picture will be generated. + 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) @@ -1050,18 +1173,38 @@ def generate_room_picture(self, room_node_id): def insert_current_edge(self, room_id): # Insert current edge to the room """ - Inserts or assigns an edge in the graph with the current room ID as the - tail and the same ID as the head, indicating that the agent is currently - located in that 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 (str): Used to represent the id of the current room in which - the agent is located. + 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) @@ -1073,13 +1216,18 @@ def update_node_att(self, id: int, attribute_names: [str]): def update_node(self, id: int, type: str): """ - Updates the node attributes based on user input and inserts new edges into - the graph according to predefined rules. + 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 to identify the node to be updated. - type (str): Used to identify the type of node being updated (door or - room). + 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": @@ -1092,34 +1240,34 @@ def update_node(self, id: int, type: str): # print("Door node found in igraph. Returning.") return except: - print("No door node found in igraph. Checking if room node exists") + # 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) + # print("Parent id", parent_id) door_parent_node = self.g.get_node(parent_id) - print("Door parent name", door_parent_node.name) + # print("Door parent name", door_parent_node.name) # Insert door node in igraph self.insert_igraph_vertex(door_node) - print("Door inserted in igraph") + # 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) + # 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") + # 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) + # print("No room node found in igraph. Not possible to insert door") + # print(e) return if id == self.affordance_node_active_id: @@ -1141,14 +1289,17 @@ def update_edge(self, fr: int, to: int, type: str): # TODO: Posible problema si tocas el nodo room en la interfaz gráfica """ - Updates an edge in the graph based on its type, source node, and target node. + 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): Representing the first room number of an edge to be updated - with new information. - to (int): Used to identify the target node in the graph for updating - the edge. - type (str): Used to indicate whether the edge is a robot or a room node. + 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: