diff --git a/mrobosub_perception/launch/perception.launch b/mrobosub_perception/launch/perception.launch index 1e28a199..3214a063 100644 --- a/mrobosub_perception/launch/perception.launch +++ b/mrobosub_perception/launch/perception.launch @@ -1,7 +1,6 @@ - - - - - + + + + diff --git a/mrobosub_perception/launch/perception_sim.launch b/mrobosub_perception/launch/perception_sim.launch deleted file mode 100644 index eda6d462..00000000 --- a/mrobosub_perception/launch/perception_sim.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/mrobosub_perception/launch/perception_sim_launch.xml b/mrobosub_perception/launch/perception_sim_launch.xml new file mode 100644 index 00000000..03386c7e --- /dev/null +++ b/mrobosub_perception/launch/perception_sim_launch.xml @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/mrobosub_perception/launch/png_pub_launch.xml b/mrobosub_perception/launch/png_pub_launch.xml new file mode 100644 index 00000000..34ab83d7 --- /dev/null +++ b/mrobosub_perception/launch/png_pub_launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/mrobosub_perception/launch/tuner.launch b/mrobosub_perception/launch/tuner_launch.xml similarity index 80% rename from mrobosub_perception/launch/tuner.launch rename to mrobosub_perception/launch/tuner_launch.xml index fc067206..2b3259b7 100644 --- a/mrobosub_perception/launch/tuner.launch +++ b/mrobosub_perception/launch/tuner_launch.xml @@ -1,13 +1,13 @@ - + /> --> diff --git a/mrobosub_perception/mrobosub_perception/bbox.png b/mrobosub_perception/mrobosub_perception/bbox.png new file mode 100644 index 00000000..73c18e3a Binary files /dev/null and b/mrobosub_perception/mrobosub_perception/bbox.png differ diff --git a/mrobosub_perception/mrobosub_perception/bin_hsv.py b/mrobosub_perception/mrobosub_perception/bin_hsv.py index 6a8e9bb5..5920ca28 100755 --- a/mrobosub_perception/mrobosub_perception/bin_hsv.py +++ b/mrobosub_perception/mrobosub_perception/bin_hsv.py @@ -6,14 +6,12 @@ from cv_bridge import CvBridge import rclpy -from rclpy import utilities -from rclpy.node import Node +from mrobosub_lib import Node from mrobosub_msgs.srv import ObjectPosition from mrobosub_perception.timed_service import TimedService from sensor_msgs.msg import Image from mrobosub_perception.hsv_pipeline import HsvPipeline -from hsv_pipeline import HsvPipeline def pixels_to_angles(frame, x_pos: int, y_pos: int, fov_x=110, fov_y=70) -> Tuple[int, int]: height, width = frame.shape[0:2] diff --git a/mrobosub_perception/mrobosub_perception/png_pub.py b/mrobosub_perception/mrobosub_perception/png_pub.py index f5a6a72d..bbc8dc4d 100755 --- a/mrobosub_perception/mrobosub_perception/png_pub.py +++ b/mrobosub_perception/mrobosub_perception/png_pub.py @@ -1,40 +1,38 @@ -#!/usr/bin/env python -import rospy # Python library for ROS +import rclpy from sensor_msgs.msg import Image # Image is the message type import cv2 # OpenCV library from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images +from mrobosub_lib import Node +import os -def publish_message(): +class PngPub(Node): + def __init__(self): + super().__init__("png_pub") + # Node is publishing to the video_frames topic using the message type Image + self.pub = self.create_publisher(Image, '/zed2/zed_node/rgb/image_rect_color', qos_profile=10) - # Node is publishing to the video_frames topic using - # the message type Image - pub = rospy.Publisher('/zed2/zed_node/rgb/image_rect_color', Image, queue_size=10) + current_dir = os.path.dirname(os.path.realpath(__file__)) + img_path = os.path.join(current_dir, "bbox.png") + self.img = cv2.imread(img_path) + if self.img is None: + print("Error: image not found!") + # when I run this, this input is currently "not a numpy array" + # but we can think about the processing here once we actually have an image we wanna publish - rospy.init_node('png_pub_py', anonymous=True) + #cap.set(cv2.CAP_PROP_EXPOSURE, -8) - # Go through the loop 10 times per second - rate = rospy.Rate(10) # 10hz + # Used to convert between ROS and OpenCV images + self.br = CvBridge() + self.timer = self.create_timer(0.1, self.publish_message) # Go through the loop 10 times per second - # Create a VideoCapture object - # The argument '0' gets the default webcam. - img = cv2.imread("./bbox.png") - #cap.set(cv2.CAP_PROP_EXPOSURE, -8) + def publish_message(self): + self.pub.publish(self.br.cv2_to_imgmsg(self.img, encoding='bgr8')) - # Used to convert between ROS and OpenCV images - br = CvBridge() - - #print(type(img)) - - # While ROS is still running. - while not rospy.is_shutdown(): - pub.publish(br.cv2_to_imgmsg(img, encoding='bgr8')) - - # Sleep just enough to maintain the desired rate - rate.sleep() +def main(): + rclpy.init() + node = PngPub() + rclpy.spin(node) if __name__ == '__main__': - try: - publish_message() - except rospy.ROSInterruptException as e: - raise e \ No newline at end of file + main() diff --git a/mrobosub_perception/setup.py b/mrobosub_perception/setup.py index 97a18e35..0142a035 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', + 'png_pub = mrobosub_perception.png_pub:main' ], }, )