IE-482-582 / spring2024

3 stars 0 forks source link

images not capturing #55

Open afifac opened 1 month ago

afifac commented 1 month ago

After running the code below, the images are not being captured on our husky. Can we get some guidance on where the problem might be to fix this issue?

    #!/usr/bin/env python3

    import rospy
    from geometry_msgs.msg import Twist
    from sensor_msgs.msg import Image
    from cv_bridge import CvBridge
    import cv2
    import os
    import math
    import numpy as np
    import tf
    from tf.transformations import euler_from_quaternion
    from PIL import Image as PILImage
    from PIL.ExifTags import TAGS

    class HuskyCircleSimulator:
        def __init__(self):
            rospy.init_node('husky_circle_simulator')
            self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
            self.image_sub = rospy.Subscriber('/realsense/camera/image_raw', Image, self.image_callback)
            self.bridge = CvBridge()
            self.image_count = 0
            self.image_folder = os.path.join('/home/student/Pictures/images')

            self.tf_listener = tf.TransformListener()

            if not os.path.exists(self.image_folder):
                os.makedirs(self.image_folder)

            self.start_x = 0.0  # Adjust to the desired starting x-coordinate
            self.start_y = 0.0  # Adjust to the desired starting y-coordinate

        def image_callback(self, data):
            cv_image = self.bridge.imgmsg_to_cv2(data, 'bgr8')
            image_path= os.path.join(self.image_folder, 'husky_image_{}.jpg'.format(self.image_count))
            cv2.imwrite(image_path, cv_image)
            self.add_exif_data(image_path)
            self.image_count += 1
            rospy.loginfo("Image captured and saved")  # Log statement to check if the callback is called

        def add_exif_data(self,image_folder):
            image = PILImage.open(image_folder)
            exif_data = image.info.get('exif', {})
            exif_data['Software'] = 'Husky Circle Simulator'
            image.save(image_folder, exif=image.info['exif'])

        def calculate_angular_velocity(self, linear_velocity, radius):
            if radius == 0:
                return 0
            else:
                return linear_velocity / radius

        def move_husky(self):
            rate = rospy.Rate(10)  # Increase the rate to 10Hz for better control
            twist = Twist()

            # After reaching the starting location, initiate circular motion
            twist.linear.x = 0.5  # Increase linear velocity for larger radius circular motion
            radius = 2.0  # Adjust the desired radius of the circular trajectory
            twist.angular.z = self.calculate_angular_velocity(twist.linear.x, radius)

            images_taken = 0

            while not rospy.is_shutdown():
                # Check if the number of images taken has reached the limit
                if images_taken == 25:
                    rospy.loginfo("Reached the limit of 25 images. Stopping.")
                    break

                self.cmd_vel_pub.publish(twist)
                rate.sleep()

                # Increment the image count every time the robot completes a full rotation
                if abs(twist.angular.z) > 0.5:  # Assuming the rotation is significant, meaning the robot has completed a rotation
                    images_taken += 1
                    rospy.loginfo("Took image {}. Total images taken: {}.".format(images_taken, images_taken))

            # Stop the robot
            twist.linear.x = 0
            twist.angular.z = 0
            self.cmd_vel_pub.publish(twist)

    if __name__ == '__main__':
        try:
            husky_simulator = HuskyCircleSimulator()
            husky_simulator.move_husky()
        except rospy.ROSInterruptException:
            pass 
Screenshot 2024-05-13 at 3 47 17 PM
afifac commented 1 month ago

Never mind, I found that in order for the images to upload '/realsense/camera/image_raw' should be called as '/realsense/color/image_raw' to run.