Add files via upload

Dockerfile

@@ -1,30 +1,2 @@
-FROM ubuntu:focal
-RUN apt update -y && apt install curl gnupg2 lsb-release -y
-RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key  -o /usr/share/keyrings/ros-archive-keyring.gpg
-RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null
-RUN apt install wget -y
-RUN wget https://github.com/ros2/ros2/releases/download/release-foxy-20210902/ros2-foxy-20210902-linux-focal-amd64.tar.bz2
-WORKDIR /robot_software_transformers-main
-COPY . .
-RUN mkdir -p /ros2_foxy
-WORKDIR /ros2_foxy
-RUN tar -xf /ros2-foxy-20210902-linux-focal-amd64.tar.bz2
-RUN apt update -y
-RUN apt install -y python3-rosdep
-RUN rosdep init
-RUN rosdep update
-RUN rosdep install --from-paths /ros2_foxy/ros2-linux/share --ignore-src --rosdistro foxy -y --skip-keys "console_bridge fastcdr fastrtps osrf_testing_tools_cpp poco_vendor rmw_connextdds rti-connext-dds-5.3.1 tinyxml_vendor tinyxml2_vendor urdfdom urdfdom_headers"
-RUN apt install -y libpython3-dev python3-pip
-RUN pip3 install -U argcomplete
-RUN apt update
-RUN apt install python3-colcon-common-extensions -y
-WORKDIR /robot_software_transformers-main
-RUN mkdir ../src
-RUN mv package.xml ../src/package.xml
-RUN mv resource ../src/resource
-RUN mv robot_software_transformers ../src/robot_software_transformers
-RUN mv setup.cfg ../src/setup.cfg
-RUN mv setup.py ../src/setup.py
-RUN mv test ../src/test
-RUN mv /src /robot_software_transformers-main
-ENTRYPOINT /bin/bash
+FROM ros2:final
+COPY . /robot_software_transformers-main/src/robot_software_transformers

robot_software_transformers/camera_node.py

@@ -9,42 +9,37 @@
 class CameraNode(Node):
 
     def __init__(self, camera_url, calib_file, map_file, cameraID):
-        super().__init__('c_'+str(cameraID))
+        super().__init__('c_'+str(cameraID)) # making a node named c_1/c_2.....
         self.cameraID = cameraID
         self.camera = Camera(camera_url)
-        self.camera._load(calib_file)
-        self.camera.load_map(map_file)
+        self.camera._load(calib_file) # loading the caliberation file into the camera object of camera.py
+        self.camera.load_map(map_file) # loading the map file into the camera object of camera.py
         self.pubs = []
-        self.camera.video.start()
+        self.camera.video.start() # starting the video caputure
 
-        for i in range(4):
-            self.pubs.append(self.create_publisher(Pose2D,"c_"+str(self.cameraID) +"/r_"+ str(i+1), 1))
+        for i in range(4): # 4 because of 4 robots
+            self.pubs.append(self.create_publisher(Pose2D,"c_"+str(self.cameraID) +"/r_"+ str(i+1), 1)) # making topics of /r_1/c_1, /r_1/c_2...
 
 
-        timer_period = 0.03
+        timer_period = 1/30
 
-        self.timer = self.create_timer(timer_period, self.timer_callback)
+        self.timer = self.create_timer(timer_period, self.timer_callback) # creating a timer object which will call timer_callback in 30 Hz
 
     def timer_callback(self):
-
-
-        poses,ids = self.camera.cord_rel_to_marker(self.camera.get_frame(),11.8)
-        msg= Pose2D()
-        for pose,id in zip(poses,ids):
-            id=id-201
-            msg.x=pose[1][0][0]
-            msg.y= pose[1][1][0]
-            msg.theta= pose[0][2][0]
-            if id[0]<4:
+        poses,ids = self.camera.cord_rel_to_marker(self.camera.get_frame(),11.8) # passing camera's frame [self.camera.get_frame()] and the size of aruco markers [11.8]     msg= Pose2D()
+        for pose,id in zip(poses,ids): # pose is the pose of the robot and id is the ID of the robot
+            id=id-201 # because aruco markers start with 200 for our specifications
+            msg.x=pose[1][0][0] # getting x coordinate
+            msg.y= pose[1][1][0] # getting y coordinate
+            msg.theta= pose[0][2][0] # getting theta
+            if id[0]<4: # checking if aruco markers are 201/202/203/204 
                 self.pubs[id[0]].publish(msg)
 
 def main():
-    # will have to initialize the camera library ?
-    # Initialize the rclpy library
-    print(sys.argv)
-    rclpy.init()
-    camera_node = CameraNode(sys.argv[1],sys.argv[2],sys.argv[3],sys.argv[4])
-    rclpy.spin(camera_node)
+    print(sys.argv) # printing arguments
+    rclpy.init() # init the rclpy
+    camera_node = CameraNode(sys.argv[1],sys.argv[2],sys.argv[3],sys.argv[4]) # making object of CameraNode
+    rclpy.spin(camera_node) # spinning the object
     rclpy.shutdown()  # Shutdown the ROS client library for Python
 
 if __name__ == '__main__':

robot_software_transformers/central_node.py

@@ -14,60 +14,64 @@ def pose_tolerance(p1,p2):
     return isEqual
 class CentralNode(Node):
 
-    def __init__(self, n_camera, robot_id):
-        super().__init__('r_'+str(robot_id))
-        self.group = MutuallyExclusiveCallbackGroup()
-        self.publisher = self.create_publisher(Pose2D, 'r_'+str(robot_id)+'/pose', 1)
+    def __init__(self, n_camera, robot_id): # passing the number of cams and robots
+        super().__init__('r_'+str(robot_id)) # making robot nodes like r_1/r_2.....
+        self.group = MutuallyExclusiveCallbackGroup() # preventing race conditions
+        self.publisher = self.create_publisher(Pose2D, 'r_'+str(robot_id)+'/pose', 1) # create pub with topic r_1/pose, r_2/pose...
         time_period = 1/30
-        self.timer = self.create_timer(time_period, self.pub_callback, callback_group=self.group)
+        self.timer = self.create_timer(time_period, self.pub_callback, callback_group=self.group) # create timer to call publisher_callback and callback group will define that the code belongs to the mutually exclusive callgroups
         self.n_camera = n_camera
         self.robot_id = robot_id
         self.subs = []
         self.camera_pose = []
         self.camera_available = dict()
         for i in range(n_camera):
-            self.subs.append(self.create_subscription(Pose2D, 'c_'+str(i+1)+'/r_'+str(robot_id), functools.partial(self.subs_callback,camera_id = i), 1, callback_group=self.group))
-            self.camera_pose.append([])
+            self.subs.append(self.create_subscription(Pose2D, 'c_'+str(i+1)+'/r_'+str(robot_id), functools.partial(self.subs_callback,camera_id = i), 1, callback_group=self.group)) # making robot nodes like r_1/r_2..... and functools.partial passes params
+            self.camera_pose.append([]) # appending empty list to cam_pose list
             self.camera_available[i] = False
         self.old_pose = None
         self.old_poses_x = []
         self.old_poses_y = []
         self.old_poses_theta = []
 
     def subs_callback(self, msg, camera_id):
-        self.camera_pose[camera_id].append(msg)
-        self.camera_available[camera_id] = True
+        self.camera_pose[camera_id].append(msg) # appending msg recieved from the topic in the list
+        self.camera_available[camera_id] = True # and setting bool to true
 
     def pub_callback(self):
         pose_holder = Pose2D()
-        pose_holder.x = 0.0
-        pose_holder.y = 0.0
-        pose_holder.theta = 0.0
-        cnt = 0
-        camera_id = -1
-        for i in range(self.n_camera):
-            if self.camera_available[i]:
+        pose_holder.x = 0.0 # init with 0
+        pose_holder.y = 0.0 # init with 0
+        pose_holder.theta = 0.0 # init with 0
+        cnt = 0 # init with 0
+        camera_id = -1 # init with -1
+        for i in range(self.n_camera): # looping through number of cams
+            if self.camera_available[i]: # check if cam is available
                 camera_id = i
-                break
-        if camera_id==-1:
+                break # temp break
+        if camera_id==-1: # if no camera then just return
             return
         for i in range(self.n_camera):
-            self.camera_available[i] = False
-        if len(self.camera_pose[camera_id]) > 0:
-            if self.old_pose is None:
+            self.camera_available[i] = False # reason to make this false? redundant?
+        if len(self.camera_pose[camera_id]) > 0: #if list not empty means cam exists
+            if self.old_pose is None: # if just init meaning no old pose
                 for pose in self.camera_pose[camera_id]:
-                    pose_holder.x += pose.x
-                    pose_holder.y += pose.y
-                    pose_holder.theta += pose.theta
-                    cnt = cnt + 1
+                    pose_holder.x += pose.x # init x coordinate (adding x to 0 basically)
+                    pose_holder.y += pose.y # init y coordinate (adding x to 0 basically)
+                    pose_holder.theta += pose.theta # init theta (adding x to 0 basically)
+                    cnt = cnt + 1 # number of block crossed + 1
             else:
                 for pose in self.camera_pose[camera_id]:
-                    if pose_tolerance(pose,self.old_pose):
-                        pose_holder.x += pose.x
-                        pose_holder.y += pose.y
-                        pose_holder.theta += pose.theta
-                        cnt = cnt + 1
+                    if pose_tolerance(pose,self.old_pose): # check for tolerance (redundant hi hai btw)
+                        pose_holder.x += pose.x # adding x to old x
+                        pose_holder.y += pose.y # adding y to old y
+                        pose_holder.theta += pose.theta # adding theta to old theta
+                        cnt = cnt + 1 # number of block crossed + 1
             if cnt>0:
+                """
+                Some bullshitery bhaiya did to implement update rejection which basically rejects any value which might be very large or very small
+                For example all values are 10,9,11,12,10 anfd suddenly there comes a value 100 or -70 or something then this code will reject it
+                """
                 pose_holder.x = pose_holder.x/cnt
                 pose_holder.y = pose_holder.y/cnt
                 pose_holder.theta = pose_holder.theta/cnt
@@ -85,31 +89,31 @@ def pub_callback(self):
                 self.old_poses_y = self.old_poses_y[max(-1*len(self.old_poses_y),-5):]
                 self.old_poses_theta = self.old_poses_theta[max(-1*len(self.old_poses_theta),-5):]
                 pose_holder = Pose2D()
-                pose_holder.x = mode(self.old_poses_x)
-                pose_holder.y = mode(self.old_poses_y)
-                pose_holder.theta = mode(self.old_poses_theta)
+                pose_holder.x = mode(self.old_poses_x) # taking mode of old x poses and putting in x
+                pose_holder.y = mode(self.old_poses_y) # taking mode of old y poses and putting in y
+                pose_holder.theta = mode(self.old_poses_theta) # taking mode of old thetas and putting in theta
                 self.publisher.publish(pose_holder)
             for i in range(self.n_camera):
-                self.camera_pose[i] = []
+                self.camera_pose[i] = [] # init a 2D list
 
 def main():
-    n_camera = int(sys.argv[1])
-    n_robots = int(sys.argv[2])
+    n_camera = int(sys.argv[1]) # getting the number of cameras
+    n_robots = int(sys.argv[2]) # getting the number of robots
     print(n_camera, n_robots)
-    rclpy.init()
+    rclpy.init() # init the rclpy
     nodes = []
     try:
-        executor = MultiThreadedExecutor(4)
+        executor = MultiThreadedExecutor(4) # init a 4 multithreadedexecutor
         for i in range(n_robots):
-            cnode = CentralNode(n_camera, i+1)
-            nodes.append(cnode)
-            executor.add_node(cnode)
+            cnode = CentralNode(n_camera, i+1) # making object of centralnode
+            nodes.append(cnode) # need to append check finally for the need
+            executor.add_node(cnode) # add the node in the executor object so that when executor does work, all the nodes in executor will spin
         try:
-            executor.spin()
+            executor.spin() # spinning the exec object
         finally:
-            executor.shutdown()
+            executor.shutdown() # shutting down the exec object
             for i in range(n_robots):
-                nodes[i].destroy_node()
+                nodes[i].destroy_node() # destorying the nodes before the final shutdown of rclpy
     finally:
         rclpy.shutdown()
 

robot_software_transformers/central_path_node.py

@@ -0,0 +1,69 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Pose2D
+from std_msgs.msg import Bool
+import functools
+
+class CentralPathNode(Node):
+    def __init__(self):
+        super().__init__('central_path_node') # making node called cpn
+        self.time_period = 1/30
+        """subscribing to all bool topics and target topics"""
+        self.r_1_bool_sub = self.create_subscription(Bool, 'r_1/Bool', self.r_1_callback, 1)
+        self.r_2_bool_sub = self.create_subscription(Bool, 'r_2/Bool', self.r_2_callback, 1)
+        self.r_3_bool_sub = self.create_subscription(Bool, 'r_3/Bool', self.r_3_callback, 1)
+        self.r_4_bool_sub = self.create_subscription(Bool, 'r_4/Bool', self.r_4_callback, 1)
+        self.r_1_pose_sub = self.create_subscription(Pose2D, 'r_1/target', self.r_1_pose_callback, 1)
+        self.r_2_pose_sub = self.create_subscription(Pose2D, 'r_2/target', self.r_2_pose_callback, 1)
+        self.r_3_pose_sub = self.create_subscription(Pose2D, 'r_3/target', self.r_3_pose_callback, 1)
+        self.r_4_pose_sub = self.create_subscription(Pose2D, 'r_4/target', self.r_4_pose_callback, 1)
+        """making a publisher to publish in final_target topics"""
+        self.r_1_pose_final = self.create_publisher(Pose2D, 'r_1/final_target', 1)
+        self.r_2_pose_final = self.create_publisher(Pose2D, 'r_2/final_target', 1)
+        self.r_3_pose_final = self.create_publisher(Pose2D, 'r_3/final_target', 1)
+        self.r_4_pose_final = self.create_publisher(Pose2D, 'r_4/final_target', 1)
+
+    def r_1_callback(self,msg):
+        if(msg.data==True):
+            """if the bool data is true meaning bot1 is active hence will work or else wont"""
+            self.time = self.create_timer(self.time_period, functools.partial(self.timer_callback,content=self.r_1_pose_sub))
+
+    def r_2_callback(self,msg):
+        if(msg.data==True):
+            """if the bool data is true meaning bot2 is active hence will work or else wont"""
+            self.time = self.create_timer(self.time_period, functools.partial(self.timer_callback,content=self.r_2_pose_sub))
+
+    def r_3_callback(self,msg):
+        if(msg.data==True):
+            """if the bool data is true meaning bot3 is active hence will work or else wont"""
+            self.time = self.create_timer(self.time_period, functools.partial(self.timer_callback,content=self.r_3_pose_sub))
+
+    def r_4_callback(self,msg):
+        if(msg.data==True):
+            """if the bool data is true meaning bot4 is active hence will work or else wont"""
+            self.time = self.create_timer(self.time_period, functools.partial(self.timer_callback,content=self.r_4_pose_sub))
+
+    def r_1_pose_callback(self,msg):
+        self.r_1_pose_sub = msg
+
+    def r_2_pose_callback(self,msg):
+        self.r_2_pose_sub = msg
+
+    def r_3_pose_callback(self,msg):
+        self.r_3_pose_sub = msg
+
+    def r_4_pose_callback(self,msg):
+        self.r_4_pose_sub = msg
+
+    def timer_callback(self, content):
+        """This function will simply recieve the Pose2D object as content and then publish it in new topic final_target"""
+        self.r_1_pose_final.publish(content)
+
+def main():
+    rclpy.init() # init rclpy
+    central_path_node = CentralPathNode() # init object of CentralPathNode
+    rclpy.spin(central_path_node) # spin the object
+    rclpy.shutdown() # shutdown
+
+if __name__ == '__main__':
+    main()