diff --git a/mrobosub_perception/launch/rectify_image.launch b/mrobosub_perception/launch/rectify_image.launch deleted file mode 100644 index aa048ac4..00000000 --- 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 00000000..341c8613 --- /dev/null +++ b/mrobosub_perception/launch/rectify_image_launch.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/mrobosub_perception/mrobosub_perception/rectify_image.py b/mrobosub_perception/mrobosub_perception/rectify_image.py index 6a2e4e54..5e38e040 100755 --- a/mrobosub_perception/mrobosub_perception/rectify_image.py +++ b/mrobosub_perception/mrobosub_perception/rectify_image.py @@ -5,11 +5,11 @@ from typing import Tuple from cv_bridge import CvBridge -import rospy -from dynamic_reconfigure.server import Server +import rclpy 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 @@ -85,16 +85,17 @@ 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_params() + self.add_post_set_parameters_callback(self.set_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_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') @@ -108,14 +109,31 @@ 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 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() + param_desc_int.type = rclpy.Parameter.Type.INTEGER + param_desc_int.description = "An int parameter" + self.declare_parameters( + namespace='', + 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.set_params() + + +def main(): + rclpy.init() + node = RectifiedImage() + rclpy.spin(node) if __name__== '__main__': - RectifiedImage().run() + main() diff --git a/mrobosub_perception/params/rectify_image.yaml b/mrobosub_perception/params/rectify_image.yaml new file mode 100644 index 00000000..cbcbf228 --- /dev/null +++ b/mrobosub_perception/params/rectify_image.yaml @@ -0,0 +1,5 @@ +rectify_image: + ros__parameters: + w: 640 + h: 480 + f: 140 diff --git a/mrobosub_perception/setup.py b/mrobosub_perception/setup.py index fc184f8c..6d681421 100644 --- a/mrobosub_perception/setup.py +++ b/mrobosub_perception/setup.py @@ -28,9 +28,9 @@ 'pathmarker_hsv= mrobosub_perception.pathmarker_hsv:main', 'ml_executor = mrobosub_perception.ml_executor:main', 'ml_srv = mrobosub_perception.ml_srv:main', - 'dummy_ml_srv_node = mrobosub_perception.dummy_ml_srv_node:main' - 'png_pub = mrobosub_perception.png_pub:main' - + 'rectify_image = mrobosub_perception.rectify_image:main', + 'dummy_ml_srv_node = mrobosub_perception.dummy_ml_srv_node:main', + 'png_pub = mrobosub_perception.png_pub:main', ], }, )