def __callback_get_obj_relative_pose(self, req):
# Reading last image
img = self.__video_stream.read_undistorted()
if img is None:
rospy.logwarn(
"Vision Node - Try to get object relative pose while stream is not running !"
)
return CommandStatus.VIDEO_STREAM_NOT_RUNNING, ObjectPose(
), "", "", CompressedImage()
# Extracting parameters from request
obj_type = ObjectType[req.obj_type]
obj_color = ColorHSV[req.obj_color]
workspace_ratio = req.workspace_ratio
ret_image = req.ret_image
# Creating ObjectDetector, an object for object detection
self.__object_detector = ObjectDetector(
obj_type=obj_type,
obj_color=obj_color,
workspace_ratio=workspace_ratio,
ret_image_bool=ret_image,
)
# Launching pipeline
status, msg_res_pos, obj_type, obj_color, im_draw = self.__object_detector.extract_object_with_hsv(
img)
if self.__object_detector.should_ret_image():
_, msg_img = generate_msg_from_image(
im_draw, compression_quality=self.__debug_compression_quality)
else:
msg_img = CompressedImage()
return status, msg_res_pos, obj_type, obj_color, msg_img
def _loop(self):
while not rospy.is_shutdown():
if not self._should_run:
self._running = False
self.__actualization_rate_no_stream.sleep()
continue
self.__video_stream = cv2.VideoCapture(self.__cam_port)
if not self.__video_stream.isOpened():
rospy.logwarn_throttle(10.0, "Vision Node - Failed to open video stream. Check camera is well plugged.")
self._running = False
if self.__last_time_read is None or \
(rospy.get_time() - self.__last_time_read) > self.__max_restart_time:
self._should_run = False
self.__actualization_rate_no_stream.sleep()
continue
rospy.loginfo("Vision Node - Video Stream Open")
self.__setup_stream_settings()
self._running = True
self._should_run = True
index_read = -1
while not rospy.is_shutdown() and self._should_run:
index_read = (index_read + 1) % self._subsample_read
with self.__lock_image:
bool_grabbed = self.__video_stream.grab()
if not bool_grabbed:
rospy.logwarn("Vision Node - Image not grabbed. Camera may have been unplugged.")
rospy.logwarn("Vision Node - Closing Video Stream")
break
if index_read == 0:
_, frame = self.__video_stream.retrieve()
self.__last_time_read = rospy.get_time()
self.__frame_raw = frame
if self._undistort_stream:
self.__frame_undistort = self._calibration_object.undistort_image(frame)
else:
self.__frame_undistort = None
used_image = self.__frame_undistort if self._undistort_stream else self.__frame_raw
if self._display:
cv2.imshow("Video Stream", used_image)
cv2.waitKey(1)
result, msg = generate_msg_from_image(used_image)
if not result:
continue
rospy.logdebug("Vision Node - Publishing an image")
try:
self._publisher_compressed_stream.publish(msg)
except rospy.ROSException:
return
self._actualization_rate_ros.sleep()
self.__video_stream.release()
rospy.loginfo("Vision Node - Video Stream Stopped")
self._running = False
def __callback_debug_markers(self, _):
img = self.__video_stream.read_undistorted()
if img is None:
rospy.logwarn_throttle(
2.0,
"Vision Node - Try to get debug markers while stream is not running !"
)
return False, CompressedImage()
markers_detected, img_res = debug_markers(img)
_, msg_img = generate_msg_from_image(
img_res, compression_quality=self.__debug_compression_quality)
return markers_detected, msg_img
def __callback_debug_color(self, req):
img = self.__video_stream.read_undistorted()
if img is None:
rospy.logwarn_throttle(
2.0,
"Vision Node - Try to get debug colors while stream is not running !"
)
return CompressedImage()
color = ColorHSV[req.color]
img_res = debug_threshold_color(img, color)
_, msg_img = generate_msg_from_image(
img_res, compression_quality=self.__debug_compression_quality)
return msg_img
def __acquisition_loop(self):
self._running = True
self._should_run = True
index_read = -1
while not rospy.is_shutdown() and self._should_run:
index_read = (index_read + 1) % self._subsample_read
with self.__lock_image:
bool_grabbed = self.__video_stream.grab()
if not bool_grabbed:
rospy.logwarn(
"Vision Node - Image not grabbed. Camera may have been unplugged."
)
rospy.logwarn("Vision Node - Closing Video Stream")
break
if index_read == 0:
_, frame = self.__video_stream.retrieve()
self.__last_time_read = rospy.get_time()
self.__frame_raw = frame
if self._undistort_stream:
self.__frame_undistort = self._calibration_object.undistort_image(
frame)
used_image = self.__frame_undistort
else:
self.__frame_undistort = None
used_image = self.__frame_raw
used_image = self.adjust_image(used_image)
if self._flip_img:
used_image = cv2.flip(frame, -1)
if self._display:
cv2.imshow("Video Stream", used_image)
cv2.waitKey(1)
result, msg = generate_msg_from_image(used_image)
if not result:
continue
rospy.logdebug("Vision Node - Publishing an image")
try:
self._publisher_compressed_stream.publish(msg)
except rospy.ROSException:
return
self._actualization_rate_ros.sleep()