Merge pull request #3 from patham9/Renames

Renamings

channels/robot.py → channels/localization.py

@@ -6,7 +6,7 @@
 import tf2_ros
 from geometry_msgs.msg import Quaternion
 
-class RobotLocalization:
+class Localization:
     def __init__(self, node, tf_buffer):
         self.node = node
         self.tf_buffer = tf_buffer

channels/main.py

@@ -4,10 +4,10 @@
 from rclpy.node import Node
 import tf2_ros
 
-from yolo import YOLODetector
-from robot import RobotLocalization
+from objectdetection import ObjectDetector
+from localization import Localization
 from semanticslam import SemanticSLAM
-from nav import Navigation
+from navigation import Navigation
 
 class MainNode(Node):
     def __init__(self):
@@ -17,10 +17,10 @@ def __init__(self):
         self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
 
         # Instantiate the helper modules.
-        self.yolo_detector = YOLODetector(self, self.tf_buffer)
-        self.robot_localization = RobotLocalization(self, self.tf_buffer)
-        self.semantic_slam = SemanticSLAM(self, self.tf_buffer, self.robot_localization, self.yolo_detector)
-        self.navigation = Navigation(self, self.semantic_slam, self.robot_localization)
+        self.object_detector = ObjectDetector(self, self.tf_buffer)
+        self.localization = Localization(self, self.tf_buffer)
+        self.semantic_slam = SemanticSLAM(self, self.tf_buffer, self.localization, self.object_detector)
+        self.navigation = Navigation(self, self.semantic_slam, self.localization)
         self.start_navigation_to_coordinate = self.navigation.start_navigation_to_coordinate
 
 def main(args=None):

channels/nav.py → channels/navigation.py

@@ -12,10 +12,10 @@
 from mettabridge import NAV_STATE_SET, NAV_STATE_BUSY, NAV_STATE_SUCCESS, NAV_STATE_FAIL
 
 class Navigation:
-    def __init__(self, node: Node, semantic_slam, robot_localization):
+    def __init__(self, node: Node, semantic_slam, localization):
         self.node = node
         self.semantic_slam = semantic_slam
-        self.robot_localization = robot_localization
+        self.localization = localization
         self.navigation_goal = None
         self.navigation_retries = 0
         self.goal_handle = None
@@ -71,7 +71,7 @@ def send_navigation_goal(self, target_cell, command):
         goal_pose.header.stamp = self.node.get_clock().now().to_msg()
         goal_pose.pose.position.x = origin_x + (cell_x * self.semantic_slam.new_resolution) + self.semantic_slam.new_resolution / 2
         goal_pose.pose.position.y = origin_y + (cell_y * self.semantic_slam.new_resolution) + self.semantic_slam.new_resolution / 2
-        goal_pose.pose.orientation = self.robot_localization.set_orientation(command)
+        goal_pose.pose.orientation = self.localization.set_orientation(command)
         self.node.get_logger().info(f"Sending goal to ({goal_pose.pose.position.x}, {goal_pose.pose.position.y})")
         if not self.action_client.wait_for_server(timeout_sec=1.0):
             self.node.get_logger().error("Action server not available!")

channels/yolo.py → channels/objectdetection.py

@@ -9,7 +9,7 @@
 from cv_bridge import CvBridge
 from rclpy.time import Time
 
-class YOLODetector:
+class ObjectDetector:
     def __init__(self, node: Node, tf_buffer):
         self.node = node
         self.tf_buffer = tf_buffer  # (not used directly here but available for later extensions)

channels/semanticslam.py

@@ -15,11 +15,11 @@
 from mettabridge import space_tick
 
 class SemanticSLAM:
-    def __init__(self, node, tf_buffer, robot_localization, yolo_detector):
+    def __init__(self, node, tf_buffer, localization, object_detector):
         self.node = node
         self.tf_buffer = tf_buffer
-        self.robot_localization = robot_localization
-        self.yolo_detector = yolo_detector
+        self.localization = localization
+        self.object_detector = object_detector
 
         # Mapping from object category to occupancy value
         self.M = {
@@ -94,8 +94,8 @@ def build_grid_periodic(self):
                 cell_value = self.get_block_occupancy(original_data, x, y, original_width, self.downsample_factor)
                 self.low_res_grid[new_idx] = cell_value
 
-        # Update robot position using the RobotLocalization module.
-        self.robot_lowres_x, self.robot_lowres_y = self.robot_localization.get_robot_lowres_position(
+        # Update robot position using the Localization module.
+        self.robot_lowres_x, self.robot_lowres_y = self.localization.get_robot_lowres_position(
             original_origin, original_resolution, self.downsample_factor)
         if 0 <= self.robot_lowres_x < self.new_width and 0 <= self.robot_lowres_y < self.new_height:
             robot_idx = self.robot_lowres_y * self.new_width + self.robot_lowres_x
@@ -107,32 +107,32 @@ def build_grid_periodic(self):
             self.node.get_logger().warn("Robot position is out of bounds in the downsampled map.")
 
         # Process detections from the YOLODetector module.
-        yolo_detections = self.yolo_detector.detections
-        depth_image = self.yolo_detector.depth_image
-        if yolo_detections is not None:
-            for out in yolo_detections:
+        object_detections = self.object_detector.detections
+        depth_image = self.object_detector.depth_image
+        if object_detections is not None:
+            for out in object_detections:
                 for detection in out:
                     scores = detection[5:]
                     class_id = int(np.argmax(scores))
                     confidence = scores[class_id]
                     if confidence > 0.5:  # Confidence threshold for semantic mapping
-                        center_x = int(detection[0] * self.yolo_detector.width)
-                        center_y = int(detection[1] * self.yolo_detector.height)
+                        center_x = int(detection[0] * self.object_detector.width)
+                        center_y = int(detection[1] * self.object_detector.height)
                         if depth_image is None:
                             self.node.get_logger().warn("Got detections but no depth image")
                             continue
                         depth_value = depth_image[center_y, center_x]
-                        category = self.yolo_detector.classes[class_id]
+                        category = self.object_detector.classes[class_id]
                         if depth_value > 0 and category in self.M:
-                            stamp = self.yolo_detector.last_image_stamp if self.yolo_detector.last_image_stamp else self.node.get_clock().now()
+                            stamp = self.object_detector.last_image_stamp if self.object_detector.last_image_stamp else self.node.get_clock().now()
                             self.node.get_logger().info(f"DEPTH DEBUG: {depth_value}")
                             # Create a point in camera coordinates.
                             camera_point = PointStamped(
                                 header=Header(stamp=stamp.to_msg(), frame_id='oakd_left_camera_frame'),
                                 point=Point(
                                     x=depth_value,
-                                    y=-(center_x - (self.yolo_detector.width / 2)) * depth_value / self.yolo_detector.fx,
-                                    z=-(center_y - (self.yolo_detector.height / 2)) * depth_value / self.yolo_detector.fy
+                                    y=-(center_x - (self.object_detector.width / 2)) * depth_value / self.object_detector.fx,
+                                    z=-(center_y - (self.object_detector.height / 2)) * depth_value / self.object_detector.fy
                                 )
                             )
                             try:

start/start_nartech.bash

@@ -23,7 +23,7 @@ if [ "$1" != "slow" ] && [ "$2" != "slow" ] && [ "$3" != "slow" ]; then
     gnome-terminal -- bash -c "sleep 10 && export LIBGL_ALWAYS_SOFTWARE=0 && export QT_QPA_PLATFORM=xcb && gz sim --render-engine ogre \$MY_WORLD; exec bash" &
     if [ "$1" != "nonace" ] && [ "$2" != "nonace" ] && [ "$3" != "nonace" ]; then
         gnome-terminal -- bash -c "sleep 20 && cd /home/nartech/NACE/ && python3 main.py world=-2 nogui; exec bash" &
-        python3 /home/nartech/nartech_ws/src/nartech_ros/channels/gui.py &
+        python3 /home/nartech/nartech_ws/src/nartech_ros/optional/gui.py &
     fi
     geany /home/nartech/NACE/input.metta &
     ros2 launch nav2_bringup tb4_simulation_launch.py slam:=True nav:=True headless:=True autostart:=True use_sim_time:=True rviz_config_file:=nartech_view.rviz world:=$MY_WORLD
@@ -33,7 +33,7 @@ else
     export LIBGL_ALWAYS_SOFTWARE=1
     if [ "$1" != "nonace" ] && [ "$2" != "nonace" ] && [ "$3" != "nonace" ]; then
         gnome-terminal -- bash -c "sleep 20 && cd /home/nartech/NACE/ && python3 main.py world=-2 nogui; exec bash" &
-        python3 /home/nartech/nartech_ws/src/nartech_ros/channels/gui.py &
+        python3 /home/nartech/nartech_ws/src/nartech_ros/optional/gui.py &
     fi
     geany /home/nartech/NACE/input.metta &
     ros2 launch nav2_bringup tb4_simulation_launch.py slam:=True nav:=True headless:=False autostart:=True use_sim_time:=True rviz_config_file:=nartech_view.rviz world:=$MY_WORLD