From edab3b33ca476141f054061f912e687ba24b662e Mon Sep 17 00:00:00 2001 From: Vivaan Singhvi Date: Sun, 19 Oct 2025 19:44:00 +0000 Subject: [PATCH 1/5] migrated rectified image --- .../mrobosub_perception/rectify_image.py | 32 +++++++++++-------- 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/mrobosub_perception/mrobosub_perception/rectify_image.py b/mrobosub_perception/mrobosub_perception/rectify_image.py index 6a2e4e5..70e0945 100755 --- a/mrobosub_perception/mrobosub_perception/rectify_image.py +++ b/mrobosub_perception/mrobosub_perception/rectify_image.py @@ -5,7 +5,7 @@ from typing import Tuple from cv_bridge import CvBridge -import rospy +import rclpy from dynamic_reconfigure.server import Server from sensor_msgs.msg import Image from mrobosub_lib.lib import Node @@ -85,16 +85,15 @@ def generate_rectify_maps(h: int, w: int, f: int) -> Tuple[np.ndarray, np.ndarra class RectifiedImage(Node): def __init__(self): super().__init__('rectified_image') - self.f = 280 + + self.declare_and_get_params() self.br = CvBridge() self.map_x, self.map_y = None, None - self.h, self.w = 640, 480 self.shape = None - self.sub = rospy.Subscriber('/dummy_botcam', Image, self.handle_frame, queue_size=1) - self.rectified_pub = rospy.Publisher(f'/rectified_image', Image, queue_size=1) - self.srv = Server(rectify_paramsConfig, self.reconfigure_callback, 'rectify_params') + self.sub = self.create_subscriber(Image, '/dummy_botcam', self.handle_frame, qos_profile=1) + self.rectified_pub = self.create_publisher(Image, f'/rectified_image', qos_profile=1) def handle_frame(self, msg): bgr_img = self.br.imgmsg_to_cv2(msg, desired_encoding='bgr8') @@ -108,14 +107,21 @@ def handle_frame(self, msg): #rectified_img = cv2.resize(rectified_img, (640, 480), interpolation=cv2.INTER_LINEAR) self.rectified_pub.publish(self.br.cv2_to_imgmsg(rectified_img, encoding='bgr8')) - def reconfigure_callback(self, config, level): - self.f = config["f"] - self.map_x, self.map_y = generate_rectify_maps(self.h, self.w, self.f) - return config - def run(self): - rospy.spin() + def declare_and_get_params(self): + self.declare_parameter("f", 280) + self.declare_parameter("h", 640) + self.declare_parameter("w", 480) + self.f = self.get_parameter('f').get_parameter_value().integer_value + self.h = self.get_parameter('h').get_parameter_value().integer_value + self.w = self.get_parameter('w').get_parameter_value().integer_value + + +def main(): + rclpy.init() + node = RectifiedImage() + rclpy.spin(node) if __name__== '__main__': - RectifiedImage().run() + main() From f7efcb1b87a9289e1ec2f476160db5aaf9ab7aba Mon Sep 17 00:00:00 2001 From: Vivaan Singhvi Date: Tue, 21 Oct 2025 23:46:49 +0000 Subject: [PATCH 2/5] added logic in the lunch files --- mrobosub_perception/launch/rectify_image.launch | 3 --- mrobosub_perception/launch/rectify_image_launch.xml | 3 +++ mrobosub_perception/mrobosub_perception/rectify_image.py | 2 +- mrobosub_perception/setup.py | 1 + 4 files changed, 5 insertions(+), 4 deletions(-) delete mode 100644 mrobosub_perception/launch/rectify_image.launch create mode 100644 mrobosub_perception/launch/rectify_image_launch.xml diff --git a/mrobosub_perception/launch/rectify_image.launch b/mrobosub_perception/launch/rectify_image.launch deleted file mode 100644 index aa048ac..0000000 --- a/mrobosub_perception/launch/rectify_image.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/mrobosub_perception/launch/rectify_image_launch.xml b/mrobosub_perception/launch/rectify_image_launch.xml new file mode 100644 index 0000000..b0b1c73 --- /dev/null +++ b/mrobosub_perception/launch/rectify_image_launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/mrobosub_perception/mrobosub_perception/rectify_image.py b/mrobosub_perception/mrobosub_perception/rectify_image.py index 70e0945..cee01c2 100755 --- a/mrobosub_perception/mrobosub_perception/rectify_image.py +++ b/mrobosub_perception/mrobosub_perception/rectify_image.py @@ -92,7 +92,7 @@ def __init__(self): self.map_x, self.map_y = None, None self.shape = None - self.sub = self.create_subscriber(Image, '/dummy_botcam', self.handle_frame, qos_profile=1) + self.sub = self.create_subscription(Image, '/dummy_botcam', self.handle_frame, qos_profile=1) self.rectified_pub = self.create_publisher(Image, f'/rectified_image', qos_profile=1) def handle_frame(self, msg): diff --git a/mrobosub_perception/setup.py b/mrobosub_perception/setup.py index 97a18e3..a18b612 100644 --- a/mrobosub_perception/setup.py +++ b/mrobosub_perception/setup.py @@ -27,6 +27,7 @@ 'pathmarker_hsv= mrobosub_perception.pathmarker_hsv:main', 'ml_executor = mrobosub_perception.ml_executor:main', 'ml_srv = mrobosub_perception.ml_srv:main', + 'rectify_image = mrobosub_perception.rectify_image:main', ], }, ) From 00b295350459e5cb1bbee5cf6435743c85720068 Mon Sep 17 00:00:00 2001 From: vivaansinghvi07 Date: Wed, 22 Oct 2025 00:59:30 +0000 Subject: [PATCH 3/5] added to the params and launch --- .../launch/rectify_image_launch.xml | 4 ++- .../mrobosub_perception/rectify_image.py | 25 +++++++++++++------ mrobosub_perception/params/rectify_image.yaml | 5 ++++ 3 files changed, 25 insertions(+), 9 deletions(-) create mode 100644 mrobosub_perception/params/rectify_image.yaml diff --git a/mrobosub_perception/launch/rectify_image_launch.xml b/mrobosub_perception/launch/rectify_image_launch.xml index b0b1c73..341c861 100644 --- a/mrobosub_perception/launch/rectify_image_launch.xml +++ b/mrobosub_perception/launch/rectify_image_launch.xml @@ -1,3 +1,5 @@ - + + + diff --git a/mrobosub_perception/mrobosub_perception/rectify_image.py b/mrobosub_perception/mrobosub_perception/rectify_image.py index cee01c2..b2ab880 100755 --- a/mrobosub_perception/mrobosub_perception/rectify_image.py +++ b/mrobosub_perception/mrobosub_perception/rectify_image.py @@ -6,10 +6,10 @@ from cv_bridge import CvBridge import rclpy -from dynamic_reconfigure.server import Server from sensor_msgs.msg import Image -from mrobosub_lib.lib import Node -from mrobosub_perception.cfg import rectify_paramsConfig +from mrobosub_lib import Node + +from rcl_interfaces.msg import ParameterDescriptor def crop_to_circle(image: np.ndarray, radius: int) -> np.ndarray: # Find the dimensions of the image @@ -86,13 +86,14 @@ class RectifiedImage(Node): def __init__(self): super().__init__('rectified_image') - self.declare_and_get_params() + self.declare_params() self.br = CvBridge() self.map_x, self.map_y = None, None self.shape = None self.sub = self.create_subscription(Image, '/dummy_botcam', self.handle_frame, qos_profile=1) + self.sub2 = self.create_subscription(Image, f'/dummy_botcam{self.f}', self.handle_frame, qos_profile=1) self.rectified_pub = self.create_publisher(Image, f'/rectified_image', qos_profile=1) def handle_frame(self, msg): @@ -108,10 +109,18 @@ def handle_frame(self, msg): self.rectified_pub.publish(self.br.cv2_to_imgmsg(rectified_img, encoding='bgr8')) - def declare_and_get_params(self): - self.declare_parameter("f", 280) - self.declare_parameter("h", 640) - self.declare_parameter("w", 480) + def declare_params(self): + param_desc_int = ParameterDescriptor() + param_desc_int.type = rclpy.Parameter.Type.INTEGER + param_desc_int.description = "An int parameter" + self.declare_parameters( + namespace='', + parameters = [ + ("f", 280, param_desc_int), + ("h", 640, param_desc_int), + ("w", 480, param_desc_int), + ] + ) self.f = self.get_parameter('f').get_parameter_value().integer_value self.h = self.get_parameter('h').get_parameter_value().integer_value self.w = self.get_parameter('w').get_parameter_value().integer_value diff --git a/mrobosub_perception/params/rectify_image.yaml b/mrobosub_perception/params/rectify_image.yaml new file mode 100644 index 0000000..c6b842a --- /dev/null +++ b/mrobosub_perception/params/rectify_image.yaml @@ -0,0 +1,5 @@ +rectify_yaml: + ros__parameters: + w: 640 + h: 480 + f: 140 From 20a11af82bfb55cfd7b2cffad19461a3ac8a79ff Mon Sep 17 00:00:00 2001 From: vivaansinghvi07 Date: Sun, 26 Oct 2025 17:34:43 +0000 Subject: [PATCH 4/5] fix :) --- mrobosub_perception/mrobosub_perception/rectify_image.py | 9 ++++----- mrobosub_perception/params/rectify_image.yaml | 2 +- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/mrobosub_perception/mrobosub_perception/rectify_image.py b/mrobosub_perception/mrobosub_perception/rectify_image.py index b2ab880..8897ee5 100755 --- a/mrobosub_perception/mrobosub_perception/rectify_image.py +++ b/mrobosub_perception/mrobosub_perception/rectify_image.py @@ -93,7 +93,6 @@ def __init__(self): self.shape = None self.sub = self.create_subscription(Image, '/dummy_botcam', self.handle_frame, qos_profile=1) - self.sub2 = self.create_subscription(Image, f'/dummy_botcam{self.f}', self.handle_frame, qos_profile=1) self.rectified_pub = self.create_publisher(Image, f'/rectified_image', qos_profile=1) def handle_frame(self, msg): @@ -115,10 +114,10 @@ def declare_params(self): param_desc_int.description = "An int parameter" self.declare_parameters( namespace='', - parameters = [ - ("f", 280, param_desc_int), - ("h", 640, param_desc_int), - ("w", 480, param_desc_int), + parameters = [ # uses the .yaml file for the actual values + ("f", 0, param_desc_int), + ("h", 0, param_desc_int), + ("w", 0, param_desc_int), ] ) self.f = self.get_parameter('f').get_parameter_value().integer_value diff --git a/mrobosub_perception/params/rectify_image.yaml b/mrobosub_perception/params/rectify_image.yaml index c6b842a..cbcbf22 100644 --- a/mrobosub_perception/params/rectify_image.yaml +++ b/mrobosub_perception/params/rectify_image.yaml @@ -1,4 +1,4 @@ -rectify_yaml: +rectify_image: ros__parameters: w: 640 h: 480 From 81208c367b36c44cbac08662fd4a54908c807845 Mon Sep 17 00:00:00 2001 From: vivaansinghvi07 Date: Sun, 26 Oct 2025 17:45:33 +0000 Subject: [PATCH 5/5] added set parameters callback --- .../mrobosub_perception/rectify_image.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/mrobosub_perception/mrobosub_perception/rectify_image.py b/mrobosub_perception/mrobosub_perception/rectify_image.py index 8897ee5..5e38e04 100755 --- a/mrobosub_perception/mrobosub_perception/rectify_image.py +++ b/mrobosub_perception/mrobosub_perception/rectify_image.py @@ -87,6 +87,7 @@ def __init__(self): super().__init__('rectified_image') self.declare_params() + self.add_post_set_parameters_callback(self.set_params) self.br = CvBridge() self.map_x, self.map_y = None, None @@ -95,6 +96,7 @@ def __init__(self): self.sub = self.create_subscription(Image, '/dummy_botcam', self.handle_frame, qos_profile=1) self.rectified_pub = self.create_publisher(Image, f'/rectified_image', qos_profile=1) + def handle_frame(self, msg): bgr_img = self.br.imgmsg_to_cv2(msg, desired_encoding='bgr8') if self.map_x is None or self.map_y is None: @@ -107,6 +109,10 @@ def handle_frame(self, msg): #rectified_img = cv2.resize(rectified_img, (640, 480), interpolation=cv2.INTER_LINEAR) self.rectified_pub.publish(self.br.cv2_to_imgmsg(rectified_img, encoding='bgr8')) + def set_params(self, _params = None): + self.f = self.get_parameter('f').get_parameter_value().integer_value + self.h = self.get_parameter('h').get_parameter_value().integer_value + self.w = self.get_parameter('w').get_parameter_value().integer_value def declare_params(self): param_desc_int = ParameterDescriptor() @@ -120,9 +126,7 @@ def declare_params(self): ("w", 0, param_desc_int), ] ) - self.f = self.get_parameter('f').get_parameter_value().integer_value - self.h = self.get_parameter('h').get_parameter_value().integer_value - self.w = self.get_parameter('w').get_parameter_value().integer_value + self.set_params() def main():