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
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?