class Observer:
def __init__(self, camera_width, camera_height):
self.camera = Camera(width=camera_width, height=camera_height)
self.image = None
def start(self):
self.camera.observe(self._callback, names='value')
self.camera.start()
def stop(self):
self.camera.stop()
def _callback(self, change):
img = change['new']
# Change BGR TO RGB HWC
self.image = img[:, :, ::-1]
def observation(self):
while self.image is None:
pass
return self.image
class JetbotCamera:
bridge = CvBridge()
def __init__(self, calib_file=''):
self.publisher = rospy.Publisher(IMAGE_TOPIC, Image, queue_size=1)
rospy.init_node(NODE_NAME)
width = rospy.get_param('~width', 640)
height = rospy.get_param('~height', 480)
rospy.logwarn(width)
self.calib_file = calib_file
self.camera = Camera(width=width,height=height)
def start(self):
self.camera.observe(self.image_proc, names='value')
self.camera.start()
rospy.spin()
def image_proc(self, change):
image_value = change['new']
cv2_image = self.bridge.cv2_to_imgmsg(image_value, 'bgr8')
self.publisher.publish(cv2_image)
def _load_camera_info(self):
with open(self.calib_file, "r") as file_handle:
calib_data = yaml.load(file_handle)
# Parse
camera_info_msg = CameraInfo()
camera_info_msg.width = calib_data["image_width"]
camera_info_msg.height = calib_data["image_height"]
camera_info_msg.K = calib_data["camera_matrix"]["data"]
camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg.R = calib_data["rectification_matrix"]["data"]
camera_info_msg.P = calib_data["projection_matrix"]["data"]
camera_info_msg.distortion_model = calib_data["distortion_model"]
return camera_info_msg
angle = 0.0
angle_last = 0.0
def execute(change):
global angle, angle_last
image = change['new']
xy = model(preprocess(image)).detach().float().cpu().numpy().flatten()
x = xy[0]
y = (0.5 - xy[1]) / 2.0
x_slider.value = x
y_slider.value = y
speed_slider.value = speed_gain_slider.value
angle = np.arctan2(x, y)
pid = angle * steering_gain_slider.value + (angle - angle_last) * steering_dgain_slider.value
angle_last = angle
steering_slider.value = pid + steering_bias_slider.value
robot.left_motor.value = max(min(speed_slider.value + steering_slider.value, 1.0), 0.0)
robot.right_motor.value = max(min(speed_slider.value - steering_slider.value, 1.0), 0.0)
execute({'new': camera.value})
camera.observe(execute, names='value')
camera.unobserve(execute, names='value')
robot.stop()
