Added comments to 31 functions across 4 files

agents/g2o_agent/src/specificworker.py

@@ -45,6 +45,20 @@
 
 class SpecificWorker(GenericWorker):
     def __init__(self, proxy_map, startup_check=False):
+        """
+        sets up a specific worker class with a unique ID, creates an RT API object
+        and visualizer, connects signals to handle node and edge updates, and
+        initializes g2o graph with visualizer.
+
+        Args:
+            proxy_map (dict): 2D grid of the environment, which is used to initialize
+                the G2O graph and store the node positions for obstacle detection
+                and mapping.
+            startup_check (bool): initializing check to be run when the SpecificWorker
+                is created, which may include running some additional initialization
+                steps for the worker instance.
+
+        """
         super(SpecificWorker, self).__init__(proxy_map)
         self.Period = 50
 
@@ -95,6 +109,11 @@ def setParams(self, params):
     @QtCore.Slot()
     def compute(self):
 
+        """
+        performs computer vision-based SLAM using RT3D, computing landmarks and
+        optimizing the pose of a robot.
+
+        """
         if self.room_initialized:
             # Get robot odometry
             if self.odometry_queue:
@@ -154,6 +173,16 @@ def compute(self):
 
     def initialize_g2o_graph(self):
         # get robot pose in room
+        """
+        Initializes a Graph2O graph for pose estimation, given an ROS robot node
+        and room node. It gets fixed robot pose, corner values from room node, and
+        adds landmarks to the Graph2O graph for optimization.
+
+        Returns:
+            bool: a successful initialization of the G2O graph with fixed pose
+            added and corner landmarks detected and added to the graph.
+
+        """
         odom_node = self.g.get_node("Shadow")
         self.odometry_node_id = odom_node.id
         room_node = self.g.get_node("room")
@@ -193,6 +222,17 @@ def initialize_g2o_graph(self):
         return True
 
     def get_displacement(self, odometry):
+        """
+        computes the total displacement, velocity, and angular displacement of an
+        object based on its odometry data, taking into account the timestamp of
+        each point in the sequence.
+
+        Args:
+            odometry (float): 3D movement of a robot or agent, which is used to
+                calculate the displacement, velocity, and acceleration along
+                different axes.
+
+        """
         desplazamiento_avance = 0
         desplazamiento_lateral = 0
         desplazamiento_angular = 0
@@ -215,6 +255,37 @@ def get_displacement(self, odometry):
         return desplazamiento_lateral, desplazamiento_avance, -desplazamiento_angular
 
     def get_covariance_matrix(self, vertex):
+        """
+        computes the covariance matrix of a given vertex in a graph using the
+        Graph2O optimization framework and its internal optimizer. The resulting
+        matrix is returned as a concatenation of non-diagonal elements from the
+        upper triangle.
+
+        Args:
+            vertex (G2O.Vertex object.): 2D or 3D vertex for which the covariance
+                matrix is to be computed.
+                
+                		- `hessian_index`: An integer index representing the vertex's
+                position in the graph.
+                		- `vertices`: A list of vertices, each representing a point in
+                the graph.
+
+        Returns:
+            `np.array`.: a matrix containing the covariance between a set of
+            vertices in a graph.
+            
+            	1/ The return value is a matrix.
+            	2/ If the `compute_marginals` method succeeds, the returned matrix
+            will have non-zero entries only off the diagonal, indicating a covariance
+            matrix.
+            	3/ The upper triangle of the matrix will contain only non-zero elements
+            below the diagonal, indicating an upper triangular matrix.
+            	4/ The resulting matrix will be a concatenation of rows from the input
+            covariances array, where each row is a list of non-zero elements.
+            	5/ The resulting matrix will have a shape of (n, n), where n is the
+            number of vertices in the graph.
+
+        """
         cov_vertices = [(vertex.hessian_index(), vertex.hessian_index())]
         covariances, covariances_result = self.g2o.optimizer.compute_marginals(cov_vertices)
         if covariances_result:
@@ -234,6 +305,19 @@ def startup_check(self):
 
     def update_node_att(self, id: int, attribute_names: [str]):
         # pass
+        """
+        updates a node's attributes with information related to robot position,
+        side speed, advance speed and timestamp, when the node id matches the
+        odometry node id.
+
+        Args:
+            id (int): identifier of the node to be updated with the odometry data,
+                which is specifically set to the value of `self.odometry_node_id`
+                within the function.
+            attribute_names ([str]): list of names of attributes to be extracted
+                from the Shadow node in the simulation.
+
+        """
         if id == self.odometry_node_id:
             odom_node = self.g.get_node("Shadow")
             odom_attrs = odom_node.attrs
@@ -242,23 +326,74 @@ def update_node_att(self, id: int, attribute_names: [str]):
 
     def update_node(self, id: int, type: str):
         # console.print(f"UPDATE NODE: {id} {type}", style='green')
+        """
+        updates the type of a node, logging a message when doing so. If the updated
+        type is "room," it initializes a graph if necessary.
+
+        Args:
+            id (int): unique identifier of the node to be updated.
+            type (str): type of node to be updated, with values of "room" and
+                "node", which trigger different actions within the function.
+
+        """
         if type == "room":
             if not self.room_initialized:
                 self.room_initialized = True if self.initialize_g2o_graph() else False
 
 
     def delete_node(self, id: int):
+        """
+        deletes a node with the specified `id`.
+
+        Args:
+            id (int): unique identifier of the node to be deleted.
+
+        """
         pass
         # console.print(f"DELETE NODE:: {id} ", style='green')
 
     def update_edge(self, fr: int, to: int, type: str):
+        """
+        updates an edge in a graph with given frame and type.
+
+        Args:
+            fr (int): source vertex index of the edge being updated.
+            to (int): edge being updated as a string value indicating its new type.
+            type (str): edge type of the edge being updated in the graph.
+
+        """
         pass
         # console.print(f"UPDATE EDGE: {fr} to {type}", type, style='green')
 
     def update_edge_att(self, fr: int, to: int, type: str, attribute_names: [str]):
+        """
+        updates the attributes of an edge based on the given values for `fr`, `to`,
+        and `type`.
+
+        Args:
+            fr (int): 1st edge to be updated in the range of [0, 2], which is used
+                as the index for accessing the relevant attribute names from the
+                `attribute_names` list.
+            to (int): type of edge attribute to be updated in the graph.
+            type (str): type of attribute that is being updated in the edge, as a
+                string value.
+            attribute_names ([str]): 0-based array of attribute names for the edge
+                being updated in the given type, as denoted by the `type` parameter.
+
+        """
         pass
         # console.print(f"UPDATE EDGE ATT: {fr} to {type} {attribute_names}", style='green')
 
     def delete_edge(self, fr: int, to: int, type: str):
+        """
+        deletes an edge in a graph based on its ID and type.
+
+        Args:
+            fr (int): 0-based index of the first vertex in the edge to be deleted.
+            to (int): 2nd node in the edge being deleted.
+            type (str): type of edge being deleted, which is passed as a string
+                to identify the type of edge for logging purposes.
+
+        """
         pass
         # console.print(f"DELETE EDGE: {fr} to {type} {type}", style='green')

components/g2o/src/genericworker.py

@@ -39,6 +39,29 @@ class GenericWorker(QtCore.QObject):
     kill = QtCore.Signal()
 
     def __init__(self, mprx):
+        """
+        establishes a worker object, setting up a mutex and timer for the GenericWorker
+        class.
+
+        Args:
+            mprx (`object` type as declared by its enclosing code.): Qt mutex
+                object that provides mutual exclusion access to the worker's
+                internal state during execution.
+                
+                		- `super(GenericWorker, self).__init__()` is called to initiate
+                the subclassing process with the parent class ` QtCore.QObject`.
+                		- `self.mutex` is a `QtCore.QMutex` instance that provides mutual
+                exclusivity for the function. It can be used to protect the worker's
+                internal state during concurrent access.
+                		- `self.Period` is an integer variable that specifies the interval
+                between the worker's execution. This interval is set to 30 in this
+                example, but it can be changed according to the application's requirements.
+                		- `self.timer` is a `QtCore.QTimer` instance that is used to
+                schedule the worker's execution at a later time. When `self. Period`
+                elapses, the `timer_timeout()` method will be called automatically
+                to initiate the worker's execution.
+
+        """
         super(GenericWorker, self).__init__()
 
 
@@ -49,13 +72,29 @@ def __init__(self, mprx):
 
     @QtCore.Slot()
     def killYourSelf(self):
+        """
+        emits the `kill` signal, which is presumably used to terminate the execution
+        of the object and any associated processes.
+
+        """
         rDebug("Killing myself")
         self.kill.emit()
 
     # \brief Change compute period
     # @param per Period in ms
     @QtCore.Slot(int)
     def setPeriod(self, p):
+        """
+        sets the time interval for which the timer will run, using the provided
+        value and updating the `Period` attribute accordingly. It then starts the
+        timer using the new period.
+
+        Args:
+            p (int): period value to be set for the object, which is then assigned
+                to the `Period` attribute and used to start the timer with the
+                corresponding duration.
+
+        """
         print("Period changed", p)
         self.Period = p
         self.timer.start(self.Period)