From cc04c877beda58269e3c995e6a36a871203e295d Mon Sep 17 00:00:00 2001 From: gcielniak Date: Fri, 26 Sep 2025 14:08:37 +0100 Subject: [PATCH] basic functionality for workshop 2 --- .../rob2002_tutorial/counter_basic.py | 47 +++++++++++ .../rob2002_tutorial/detector_basic.py | 80 +++++++++++++++++++ .../rob2002_tutorial/mover_basic.py | 38 +++++++++ .../rob2002_tutorial/mover_laser.py | 50 ++++++++++++ src/rob2002_tutorial/setup.py | 5 -- 5 files changed, 215 insertions(+), 5 deletions(-) create mode 100644 src/rob2002_tutorial/rob2002_tutorial/counter_basic.py create mode 100644 src/rob2002_tutorial/rob2002_tutorial/detector_basic.py create mode 100644 src/rob2002_tutorial/rob2002_tutorial/mover_basic.py create mode 100644 src/rob2002_tutorial/rob2002_tutorial/mover_laser.py diff --git a/src/rob2002_tutorial/rob2002_tutorial/counter_basic.py b/src/rob2002_tutorial/rob2002_tutorial/counter_basic.py new file mode 100644 index 0000000..47ab852 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/src/rob2002_tutorial/rob2002_tutorial/detector_basic.py b/src/rob2002_tutorial/rob2002_tutorial/detector_basic.py new file mode 100644 index 0000000..4e8c3d0 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/src/rob2002_tutorial/rob2002_tutorial/mover_basic.py b/src/rob2002_tutorial/rob2002_tutorial/mover_basic.py new file mode 100644 index 0000000..a4ec94d --- /dev/null +++ b/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() \ No newline at end of file diff --git a/src/rob2002_tutorial/rob2002_tutorial/mover_laser.py b/src/rob2002_tutorial/rob2002_tutorial/mover_laser.py new file mode 100644 index 0000000..4eb0730 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/src/rob2002_tutorial/setup.py b/src/rob2002_tutorial/setup.py index ea09637..609caa0 100644 --- a/src/rob2002_tutorial/setup.py +++ b/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', ], }, )