Merge pull request #29 from LCAS/workshop_2

basic functionality for workshop 2

Author: gcielniak

src/rob2002_tutorial/rob2002_tutorial/counter_basic.py

@@ -0,0 +1,47 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import PolygonStamped
+
+class CounterBasic(Node):
+
+    def __init__(self):
+        super().__init__('counter_basic')
+
+        self.current_stamp = None
+        self.object_counter = 0
+        self.frame_counter = -1
+
+        self.subscriber = self.create_subscription(PolygonStamped, "/object_polygon", self.total_counter_callback, 10)
+
+    def counter_callback(self, data):
+        
+        if data.header.stamp != self.current_stamp: # new frame detected
+            # report the current object count
+            if self.frame_counter >= 0:
+                print(f'frame {self.frame_counter}: {self.object_counter} objects counted.')            
+        
+            self.frame_counter += 1 # increment frame counter
+            self.object_counter = 0 # reset object counter
+            self.current_stamp = data.header.stamp # refresh the current time stamp           
+
+        # keep counting objects
+        self.object_counter += 1
+
+    def total_counter_callback(self, data):       
+        # keep counting objects
+        self.object_counter += 1
+        print(f'{self.object_counter:d} objects counted so far.')
+
+def main(args=None):
+    rclpy.init(args=args)
+    counter_basic = CounterBasic()
+
+    rclpy.spin(counter_basic)
+
+    counter_basic.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rob2002_tutorial/rob2002_tutorial/detector_basic.py

@@ -0,0 +1,80 @@
+import rclpy
+from rclpy.node import Node
+from rclpy import qos
+
+import cv2 as cv
+
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Polygon, PolygonStamped, Point32
+from cv_bridge import CvBridge
+
+class DetectorBasic(Node):
+    visualisation = True
+    data_logging = True
+    log_path = 'evaluation/data/'
+    seq = 0
+
+    def __init__(self):    
+        super().__init__('detector_basic')
+        self.bridge = CvBridge()
+
+        self.min_area_size = 100.0
+        self.countour_color = (255, 255, 0) # cyan
+        self.countour_width = 1 # in pixels
+
+        self.object_pub = self.create_publisher(PolygonStamped, '/object_polygon', 10)
+        self.image_sub = self.create_subscription(Image, '/limo/depth_camera_link/image_raw', 
+                                                  self.image_color_callback, qos_profile=qos.qos_profile_sensor_data)
+        
+    def image_color_callback(self, data):
+        bgr_image = self.bridge.imgmsg_to_cv2(data, "bgr8") # convert ROS Image message to OpenCV format
+
+        # detect a color blob in the color image
+        # provide the right range values for each BGR channel (set to red bright objects)
+        bgr_thresh = cv.inRange(bgr_image, (0, 0, 80), (50, 50, 255))
+
+        # finding all separate image regions in the binary image, using connected components algorithm
+        bgr_contours, _ = cv.findContours( bgr_thresh,
+            cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE)
+
+        detected_objects = []
+        for contour in bgr_contours:
+            area = cv.contourArea(contour)
+            # detect only large objects
+            if area > self.min_area_size:
+                # get the bounding box of the region
+                bbx, bby, bbw, bbh = cv.boundingRect(contour)
+                # append the bounding box of the region into a list
+                detected_objects.append(Polygon(points = [Point32(x=float(bbx), y=float(bby)), Point32(x=float(bbw), y=float(bbh))]))
+                if self.visualisation:
+                    cv.rectangle(bgr_image, (bbx, bby), (bbx+bbw, bby+bbh), self.countour_color,  self.countour_width)
+
+        # publish individual objects from the list
+        # the header information is taken from the Image message
+        for polygon in detected_objects:
+            self.object_pub.publish(PolygonStamped(polygon=polygon, header=data.header))
+
+        # log the processed images to files
+        if self.data_logging:
+            cv.imwrite(self.log_path + f'colour_{self.seq:06d}.png', bgr_image)
+            cv.imwrite(self.log_path + f'mask_{self.seq:06d}.png', bgr_thresh)
+
+        # visualise the image processing results    
+        if self.visualisation:
+            cv.imshow("colour image", bgr_image)
+            cv.imshow("detection mask", bgr_thresh)
+            cv.waitKey(1)
+
+        self.seq += 1
+
+def main(args=None):
+    rclpy.init(args=args)
+    detector_basic = DetectorBasic()
+    
+    rclpy.spin(detector_basic)
+    
+    detector_basic.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rob2002_tutorial/rob2002_tutorial/mover_basic.py

@@ -0,0 +1,38 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+
+class MoverBasic(Node):
+
+    def __init__(self):
+        super().__init__('mover_basic')
+        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
+        timer_period = 2.0  # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+        self.i = 0
+
+    def timer_callback(self):
+        msg = Twist()
+        msg.linear.x = 0.1
+        self.publisher_.publish(msg)
+        self.get_logger().info('Publishing: "{0}"'.format(msg))
+        self.i += 1
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    mover_basic = MoverBasic()
+
+    rclpy.spin(mover_basic)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    mover_basic.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rob2002_tutorial/rob2002_tutorial/mover_laser.py

@@ -0,0 +1,50 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import LaserScan
+
+class MoverLaser(Node):
+    """
+    A very simple Roamer implementation for LIMO.
+    It goes straight until any obstacle is within
+    a specified distance and then just turns left.
+    A purely reactive approach.
+    """
+    def __init__(self):
+        """
+        On construction of the object, create a Subscriber
+        to listen to lasr scans and a Publisher to control
+        the robot
+        """
+        super().__init__('mover_laser')
+        self.publisher = self.create_publisher(Twist, "/cmd_vel", 10)
+        self.subscriber = self.create_subscription(LaserScan, "/scan", self.laserscan_callback, 10)
+
+        self.angular_range = 10
+    
+    def laserscan_callback(self, data):
+        """
+        Callback called any time a new laser scan become available
+        """
+        min_dist = min(data.ranges[int(len(data.ranges)/2) - self.angular_range : int(len(data.ranges)/2) + self.angular_range])
+        print(f'Min distance: {min_dist:.2f}')
+        msg = Twist()
+        if min_dist < 0.5:
+            msg.angular.z = 0.5
+        else:
+            msg.linear.x = 0.2
+        self.publisher.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    mvoer_laser = MoverLaser()
+    rclpy.spin(mvoer_laser)
+
+    mvoer_laser.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/rob2002_tutorial/setup.py

@@ -25,13 +25,8 @@
         'console_scripts': [
             'mover_basic = rob2002_tutorial.mover_basic:main',
             'mover_laser = rob2002_tutorial.mover_laser:main',
-            'mover_spinner = rob2002_tutorial.mover_spinner:main',
-            'mover_waypoints = rob2002_tutorial.mover_waypoints:main',
             'detector_basic = rob2002_tutorial.detector_basic:main',
-            'detector_better = rob2002_tutorial.detector_dblcounting:main',
-            'detector_3d = rob2002_tutorial.detector_3d:main',
             'counter_basic = rob2002_tutorial.counter_basic:main',
-            'counter_3d = rob2002_tutorial.counter_3d:main',
         ],
     },
 )