def test_encode_decode_cv2_compressed(self):
import numpy as np
# from: http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
formats = ["jpg", "jpeg", "jpe", "png", "bmp", "dib", "ppm", "pgm", "pbm",
"jp2", "sr", "ras", "tif", "tiff"] # this formats rviz is not support
cvb_en = CvBridge()
cvb_de = CvBridge()
for w in range(100, 800, 100):
for h in range(100, 800, 100):
for f in formats:
for channels in ([], 1, 3):
if channels == []:
original = np.uint8(np.random.randint(0, 255, size=(h, w)))
else:
original = np.uint8(np.random.randint(0, 255, size=(h, w, channels)))
compress_rosmsg = cvb_en.cv2_to_compressed_imgmsg(original, f)
newimg = cvb_de.compressed_imgmsg_to_cv2(compress_rosmsg)
self.assert_(original.dtype == newimg.dtype)
if channels == 1:
# in that case, a gray image has a shape of size 2
self.assert_(original.shape[:2] == newimg.shape[:2])
else:
self.assert_(original.shape == newimg.shape)
self.assert_(len(original.tostring()) == len(newimg.tostring()))
class PoseEstimator():
def __init__(self):
self.time_finished_processing = rospy.Time(0)
# Set up the CV Bridge
self.bridge = CvBridge()
# Square Target Classifier
''' The below...
'''
sign_cascade_file_S = str(rospy.get_param("~cascade_file_S"))
self.sign_cascade_S = cv2.CascadeClassifier(sign_cascade_file_S)
# Triangle Target Classifier
''' The below...
'''
sign_cascade_file_T = str(rospy.get_param("~cascade_file_T"))
self.sign_cascade_T = cv2.CascadeClassifier(sign_cascade_file_T)
# Load in parameters from ROS - for BLUE
self.param_use_compressed = rospy.get_param("~use_compressed", False)
self.param_target_diam = rospy.get_param("~target_diam", 1.0)
# Set additional camera parameters
self.got_camera_info = False
self.camera_matrix = None
self.dist_coeffs = None
# Set up the publishers, subscribers and tf2
self.sub_info = rospy.Subscriber("~camera_info", CameraInfo, self.callback_info)
if self.param_use_compressed:
self.sub_img = rospy.Subscriber("~image_raw/compressed", CompressedImage, self.callback_info, queue_size=10)
self.pub_overlay = rospy.Publisher("~overlay/image_raw/compressed", CompressedImage, queue_size=1)
else:
self.sub_img = rospy.Subscriber("~image_raw", Image, self.callback_info, queue_size=10)
self.pub_overlay = rospy.Publisher("~overlay/image_raw", Image, queue_size=1)
# Generate the model for the pose solver.
# For this example, draw a square around where the circle should be.
# There are 5 points, one in the centre, and one in each corner
# taken, 0.0, 0.0, 0.0 out from the first part.
''' The below...
'''
squareLength = self.param_target_diam
self.model_object = np.array([(0.0, 0.0, 0.0),
(-squareLength/2, squareLength/2, 0.0),
(squareLength/2, squareLength/2, 0.0),
(squareLength/2, -squareLength/2, 0.0),
(-squareLength/2, -squareLength/2, 0.0)])
''' The below...
'''
self.broadcaster_S = tf2_ros.TransformBroadcaster()
self.pub_found_S = rospy.Publisher('/emulated_uav/Square', Time, queue_size=10)
self.broadcaster_T = tf2_ros.TransformBroadcaster()
self.pub_found_T = rospy.Publisher('/emulated_uav/Triangle', Time, queue_size=10)
def shutdown(self):
# Unregister anything that needs it here
self.sub_info.unregister()
self.sub_img.unregister()
def callback_info(self, msg_in):
# Collect in the camera characteristics
self.dist_coeffs = np.([[msg_in.D[0], msg_in.D[1], msg_in.D[2], msg_in.D[3], msg_in.D[4]]], dtype="double")
self.camera_matrix = np.arry([(msg_in.P[0], msg_in.P[1], msg_in.P[2]),
(msg_in.P[4], msg_in.P[5], msg_in.P[6]),
(msg_in.P[9], msg_in.P[9], msg_in.P[10]))],
dtype="double")
if not self.got_camera_info:
rospy.loginfo("Got camera info")
self.got_camera_info = True
def callback_img(self, msg_in):
if msg_in.header.stamp > self.time_finished_processing:
# Don't bother to process image if we don't have camera calibration.
if self.got_camera_info:
# Convert ROS image to CV image
cv_image = None
try:
if self.param_use_compressed:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_in, "bgr8")
else:
cv_image = self.bridge.imgmsg_to_cv2(msg_in, "bgr8")
except CvBridgeError as e:
rospy.loginfo(e)
return
# If a square was detected:
''' The below...
'''
sign_S = self.sign_cascade_S.detectMultiScale(cv_image, 1.01,1)
sign_T = self.sign_cascade_T.detectMultiScale(cv_image, 1.01,1)
if sign_S:
for (x,y,w,h) in sign_S:
# Calculate the pictured model for the pose solver
# For this example, draw a square around where the circle
# should be. There are 5 points, one in the center, and
# one in each corner.
self.mode_image_S = np.array([(x+(w/2), y+(h/2)),
(x, y),
(x+w, y),
(x, y+h),
(x+w, y+h)], dtype=np.float32)])
# Do the SolvePnP method.
(success, rvec_S, tvec_S) = cv2.solvePnP(self.model_object, self.mode_image_S, self.camera_matrix, self.dist_coeffs)
# If a result was found, send to TF2
if success:
# broadcaster_S = tf2.ros.TransformBroadcaster()
# pub_found_S = rospy.Publisher('/emulated_uav/Square', Time, queue_size=10)
time_found_S = rospy.Time.now()
S = TransformStamped()
S.header.stamp = time_found_S
S.header.frame_id = "camera"
S.child_frame_id = "Square"
S.transform.translation.x = tvec_S[0]
S.transform.translation.y = tvec_S[1]
S.transform.translation.z = tvec_S[2]
S.transform.rotation.x = 0
S.transform.rotation.y = 0
S.transform.rotation.z = 0
S.transform.rotation.w = 0
self.broadcaster_S.sendTransform(S)
self.pub_found_S.publish(time_found_S)
# Draw the circle for the overlay.
cv2.rectangle(cv_image, (x,y), (x+w,y+h), (0,140,255), 2)
''' The below...
'''
if sign_T:
for (x,y,w,h) in sign_T:
# Calculate the pictured model for the pose solver
# For this example, draw a square around where the circle
# should be. There are 5 points, one in the center, and
# one in each corner.
self.mode_image_T = np.array([(x+(w/2), y+(h/2)),
(x, y),
(x+w, y),
(x, y+h),
(x+w, y+h)], dtype=np.float32)])
# Do the SolvePnP method.
(success, rvec_T, tvec_T) = cv2.solvePnP(self.model_object, self.mode_image_T, self.camera_matrix, self.dist_coeffs)
# If a result was found, send to TF2
if success:
# broadcaster_T = tf2.ros.TransformBroadcaster()
# pub_found_T = rospy.Publisher('/emulated_uav/Triangle', Time, queue_size=10)
time_found_T = rospy.Time.now()
T = TransformStamped()
T.header.stamp = time_found_T
T.header.frame_id = "camera"
T.child_frame_id = "Triangle"
T.transform.translation.x = tvec_T[0]
T.transform.translation.y = tvec_T[1]
T.transform.translation.z = tvec_T[2]
T.transform.rotation.x = 0
T.transform.rotation.y = 0
T.transform.rotation.z = 0
T.transform.rotation.w = 0
self.broadcaster_T.sendTransform(S)
self.pub_found_T.publish(time_found_T)
# Draw the circle for the overlay.
cv2.rectangle(cv_image, (x,y), (x+w,y+h), (255,0,0), 2)
# Convert CV image to ROS image and publish the mask/overlay
try:
if self.param_use_compressed:
self.pub_overlay.publish(self.bridge.cv2_to_compressed_imgmsg(cv_image, "png"))
else:
self.pub_overlay.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except (CvBridgeError, TypeError) as e:
rospy.loginfo(e)
self.time_finished_processing = rospy.Time.now()
class DataCollector:
speedAxisValue = 0.0
angleAxisValue = 0.0
STORAGE_DIR = os.path.expanduser('~') + '/data_dump/'
bridge = CvBridge()
data_writer = None
csv_file = None
csv_field_names = [
'speed', 'angle', 'high_level_command', 'image_path',
'ackerman_timestamp', 'img_timestamp', 'angle_w_noise'
]
folder_name = None
joystick_subscriber = None
angle_publisher = None
hlc_timer = None
joy_throttle_timer = None
is_joy_callback_throttled = False
# Steering noise
noise_enabled = False
current_noise_angle = 0
current_noise_peak = 0
noise_add = 0
current_noise = 0
current_angle_with_noise = 0
def __init__(self):
print 'Initializing datacollector.'
self.current_speed = 0
self.current_angle = 0
self.current_ackermann_timestamp = 0
self.recording = False
self.setup_collection_folders()
self.csv_file = open(self.folder_name + 'data.csv', 'w+')
self.data_writer = csv.writer(self.csv_file)
self.data_writer.writerow(self.csv_field_names)
self.high_level_command = HLC_FOLLOW_LANE
self.frame_seq_number = 0
self.bridge = CvBridge()
self.joystick_subscriber = rospy.Subscriber('/joy', Joy,
self._joy_callback)
self._initPubSub()
print("Spurv Joystick Driver initialized")
self._initThd()
def _initThd(self):
"""
Initializes and starts the threads
"""
self.thd = Thread(target=self._thdLoop)
self.thd.daemon = True
self.thd.start()
def _initPubSub(self):
"""
Initialize publishers and subscribers
"""
self.ackermannPub = rospy.Publisher("/ackermann_cmd",
AckermannDriveStamped,
queue_size=1)
def setup_collection_folders(self):
self.folder_name = self.STORAGE_DIR \
+ datetime.now().replace(microsecond=0).isoformat() + '/'
safe_makedirs(self.folder_name)
safe_makedirs(self.folder_name + 'images/')
def start_collecting(self):
print 'Starting data collection. Listening for commands and camera.'
self.recording = True
self.image_subscription = rospy.Subscriber(
'/fwd_camera/image_raw/compressed', CompressedImage,
self._image_callback)
def stop_collecting(self):
print 'Stopping data collection. Press Start to continue collecting.'
self.recording = False
self.image_subscription.unregister()
def get_image_path(self):
return self.folder_name + 'images/' \
+ str(self.frame_seq_number) + '.jpg'
def _image_callback(self, data_loc):
if not self.recording:
return
img_to_save = self.bridge.compressed_imgmsg_to_cv2(data_loc, 'bgr8')
img_to_save = cv2.resize(img_to_save, (0, 0),
fx=IMAGE_SCALE,
fy=IMAGE_SCALE)
img_path = self.get_image_path()
img_timestamp = data_loc.header.stamp
self.data_writer.writerow([
self.current_speed, self.current_angle, self.high_level_command,
img_path, self.current_ackermann_timestamp, img_timestamp,
self.current_angle_with_noise
])
cv2.imwrite(img_path, img_to_save)
self.frame_seq_number += 1
if self.frame_seq_number % 50 == 0:
print 'frame_seq_number = ' + str(self.frame_seq_number) \
+ ', speed = ' + str(self.current_speed) + ', angle = ' \
+ str(self.current_angle) + ', high_level_command = ' \
+ str(self.high_level_command) + '.'
def set_or_reset_hlc_timeout(self, timeout, fn):
if self.hlc_timer:
self.hlc_timer.cancel()
self.hlc_timer = Timer(timeout, fn)
self.hlc_timer.start()
def remove_joy_throttle(self):
self.is_joy_callback_throttled = False
def _joy_callback(self, joy_message):
speed_axis = joy_message.axes[SPEED_AXIS]
angle_axis = joy_message.axes[SERVO_AXIS]
record_button = joy_message.buttons[RECORD_BUTTON]
left_hlc_button = joy_message.buttons[LEFT_HLC_BUTTON]
right_hlc_button = joy_message.buttons[RIGHT_HLC_BUTTON]
straight_hlc_button = joy_message.buttons[STRAIGHT_HLC_BUTTON]
follow_hlc_button = joy_message.buttons[FOLLOW_LANE_HLC_BUTTON]
toggle_noise_button = joy_message.buttons[TOGGLE_NOISE_BUTTON]
has_pressed_relevant_button = bool(record_button) \
or bool(left_hlc_button) or bool(right_hlc_button) or bool(toggle_noise_button) \
or bool(follow_hlc_button) or bool(straight_hlc_button)
# Save speed ang angle values for thread
self.speedAxisValue = speed_axis
self.angleAxisValue = angle_axis
if not has_pressed_relevant_button or self.is_joy_callback_throttled:
return
if bool(toggle_noise_button):
self.noise_enabled = not self.noise_enabled
self.joy_throttle_timer = Timer(JOY_THROTTLE_SECONDS,
self.remove_joy_throttle)
self.joy_throttle_timer.start()
self.is_joy_callback_throttled = True
if bool(record_button):
if self.recording:
self.stop_collecting()
else:
self.start_collecting()
if bool(left_hlc_button):
print 'Setting high_level_command = HLC_LEFT.'
self.high_level_command = HLC_LEFT
self.set_or_reset_hlc_timeout(HLC_RESET_SECONDS, self._reset_hlc)
elif bool(right_hlc_button):
print 'Setting high_level_command = HLC_RIGHT.'
self.high_level_command = HLC_RIGHT
self.set_or_reset_hlc_timeout(HLC_RESET_SECONDS, self._reset_hlc)
elif bool(straight_hlc_button):
print 'Setting high_level_command = HLC_STRAIGHT.'
self.high_level_command = HLC_STRAIGHT
self.set_or_reset_hlc_timeout(HLC_RESET_SECONDS, self._reset_hlc)
elif bool(follow_hlc_button):
self._reset_hlc()
def _reset_hlc(self):
print 'Resetting high_level_command to follow'
self.high_level_command = HLC_FOLLOW_LANE
def publishAckermann(self, speed, angle):
angleScaled = self._map_range(angle, -1.0, 1.0, -1.0, 1.0)
speedScaled = self._map_range(speed, -1.0, 1.0, -MAX_SPEED, MAX_SPEED)
self.current_angle = angleScaled
self.current_speed = speedScaled
self.current_ackermann_timestamp = rospy.Time.now().to_nsec()
if self.noise_enabled:
if self.current_noise_peak == 0:
rand = (random.random() * 2 - 1)
pos = 1 if rand > 0 else -1
self.current_noise_peak = (MAX_NOISE * rand) + MIN_NOISE * pos
self.noise_add = self.current_noise_peak / 10
self.current_noise = 0
# print("Current noise peak: " + str(self.current_noise_peak))
self.current_noise += self.noise_add
if abs(self.current_noise - self.current_noise_peak) < abs(
self.current_noise_peak) * 0.01:
self.noise_add *= -1
if 0.005 > self.current_noise > -0.005:
self.current_noise_peak = 0
self.nosie_add = 0
# print("Current noise: " + str(self.current_noise))
self.current_angle_with_noise = angle + self.current_noise
msg = AckermannDriveStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.drive.steering_angle = self.current_angle_with_noise
msg.drive.speed = speedScaled
msg.drive.acceleration = max_accel_x
msg.drive.jerk = max_jerk_x
self.ackermannPub.publish(msg)
def _thdLoop(self):
while True:
self.publishAckermann(self.speedAxisValue, self.angleAxisValue)
sleep(0.10)
def _map_range(
self,
x,
in_min,
in_max,
out_min,
out_max,
):
"""
Remaps the values to a new range
"""
if x is 0:
return 0
out = (x - in_min) * (out_max - out_min) / (in_max - in_min) \
+ out_min
if out > out_max:
return out_max
elif out < out_min:
return out_min
return out
class ImageProcess:
def __init__(self):
rospy.Subscriber('/barc_cam/image_raw/compressed',
CompressedImage,
self.image_proc,
queue_size=1,
buff_size=2**24)
self.pub = rospy.Publisher('/proc_image', Image, queue_size=1)
self.lane_pub = rospy.Publisher('/lane_loc', Lanepoints, queue_size=1)
self.offset_pub = rospy.Publisher('lane_offset', Int32, queue_size=1)
self.Bridge = CvBridge()
# camera parameters
# rosparam.load_file('/home/aj/Desktop/barc_calibrationdata/ost.yaml')
self.cam_mtx = rosparam.get_param('camera_matrix/data')
self.cam_rows = rosparam.get_param('camera_matrix/rows')
self.cam_cols = rosparam.get_param('camera_matrix/cols')
self.cam_mtx = np.array(
np.array(self.cam_mtx).reshape(self.cam_rows, self.cam_cols))
self.dst_mtx = rosparam.get_param('distortion_coefficients/data')
(self.dst_rows,
self.dst_cols) = (rosparam.get_param('distortion_coefficients/rows'),
rosparam.get_param('distortion_coefficients/cols'))
self.dst_mtx = np.array(
np.array(self.dst_mtx).reshape(self.dst_rows, self.dst_cols))
# ~camera paramters
self.mono = monocular.Monocular(self.cam_mtx.T, 0.0762, -1.0, 0.0, 0.0,
np.array([0.0, 0.0]))
def h_transform(self, u, v, H):
tx = (H[0, 0] * u + H[0, 1] * v + H[0, 2])
ty = (H[1, 0] * u + H[1, 1] * v + H[1, 2])
tz = (H[2, 0] * u + H[2, 1] * v + H[2, 2])
px = (tx / tz)
py = (ty / tz)
Z = (1 / tz)
return (px, py)
def image_proc(self, data):
try:
cv_img = self.Bridge.compressed_imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# cv_img = cv2.imread('/home/aj/Curved-Lane-Lines/test_images/prescan2.jpg')
cv_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2RGB)
cv_img = cv2.undistort(cv_img, self.cam_mtx, self.dst_mtx, None)
hls = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HLS)
gray = cv2.cvtColor(cv_img, cv2.COLOR_RGB2GRAY)
r_channel = cv_img[:, :, 0]
g_channel = cv_img[:, :, 1]
b_channel = cv_img[:, :, 2]
l_channel = hls[:, :, 1]
s_channel = hls[:, :, 2]
h_channel = hls[:, :, 0]
homography = self.mono.tformToImage()
horizon = [
int(homography[0, 0] / homography[0, 2]),
int(homography[0, 1] / homography[0, 2])
]
# cv2.imshow('l_channel', l_channel)
# cv2.imshow('s_channel', s_channel)
# cv2.imshow('h_channel', h_channel)
# cv2.imshow('canny',canny)
# cv2.waitKey(0)
# l_channel[()]
gray[:horizon[1] + 30, :] = 0
gray[(gray <= 200)] = 0
# l_channel = gray*l_channel
s_channel[(s_channel <= 45)] = 0
s_channel[:s_channel.shape[0] // 2, :] = 3
l_channel[(l_channel <= 40) | (l_channel >= 55)] = 0
l_channel[:l_channel.shape[0] // 2 + 15, :] = 0
# canny = cv2.Canny(r_channel, 30, 95)
# canny[:canny.shape[0]//2,:] = 0
# cv2.imshow('s_channel',gray)
# cv2.waitKey(0)
# h_channel[(h_channel != 0)] = 1
height, width = cv_img.shape[0], cv_img.shape[1]
# gray
s_binary = np.zeros_like(gray)
s_binary[(s_channel !=
0)] = 1 # filter the region of interest from the image
# s_binary_3 = np.dstack((s_binary,s_binary,s_binary))
# test = cv_img*s_binary_3
test = gray * s_binary
test1d = np.multiply(gray, s_binary)
new_binary = np.zeros_like(test)
new_binary[(test > 182)] = 255
# cv2.imshow('s_channel', s_channel)
# cv2.imshow('l_channel', new_binary)
# cv2.imshow('s_channel', cv_img)
# cv2.waitKey(0)
# slope1 = float(height-horizon[1])/float(38-horizon[0])
# slope2 = float(height-horizon[1])/float(width-horizon[0])
# x1 = width+(int((height-150)/slope1))
# x2 = int((height-150)/slope2)
# src = np.float32([(x1, height-150), (x2, height-150),
# (230, height), (width, height)])
# dst = np.float32([(x1, height-150), (x2, height-150),
# (x1, height), (x2, height)])
# warping
src = np.float32([(180, 297), (450, 297), (13, 350), (634, 350)])
dst = np.float32([(180, 297), (450, 297), (180, 350), (450, 350)])
mat = cv2.getPerspectiveTransform(src, dst)
inv_mat = cv2.getPerspectiveTransform(dst, src)
limit = self.h_transform(horizon[0], horizon[1], mat)
warped_img = cv2.warpPerspective(cv_img, mat,
(cv_img.shape[1], cv_img.shape[0]))
warped_mask = cv2.warpPerspective(new_binary, mat,
(cv_img.shape[1], cv_img.shape[0]))
# warped_canny = cv2.warpPerspective(canny ,mat, (cv_img.shape[1], cv_img.shape[0]))
hist = np.sum(warped_mask[:, :][:warped_mask.shape[0], ], axis=0)
# cv2.imshow('warped_mask', warped_img)
# cv2.waitKey(0)
# plt.plot(hist)
# plt.show()
midpoint = int(hist.shape[0] / 2)
left_x_base = midpoint // 2 + 10 + np.argmax(
hist[midpoint // 2 + 10:midpoint])
right_x_base = np.argmax(hist[midpoint:]) + midpoint
#sliding window & curve fitting
left_a, left_b, left_c = [], [], []
right_a, right_b, right_c = [], [], []
nwindows = 9
margin = 40
minpix = 1
draw_windows = False
left_fit_ = np.empty(3)
right_fit_ = np.empty(3)
windows_height = np.int(warped_mask.shape[0] / nwindows)
nonzero = warped_mask.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
left_x_current = left_x_base
right_x_current = right_x_base
left_lane_inds = []
right_lane_inds = []
for window in range(nwindows):
win_y_low = warped_mask.shape[0] - (window + 1) * windows_height
win_y_high = warped_mask.shape[0] - window * windows_height
win_xleft_low = left_x_current - margin
win_xleft_high = left_x_current + margin
win_xright_low = right_x_current - margin
win_xright_high = right_x_current + margin
if draw_windows:
cv2.rectangle(warped_mask, (win_xleft_low, win_y_low),
(win_xleft_high, win_y_high), (100, 255, 255), 3)
cv2.rectangle(warped_mask, (win_xright_low, win_y_low),
(win_xright_high, win_y_high), (100, 255, 255),
3)
# identify the nonzero pixels in x and y within windows
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high)
& (nonzerox >= win_xleft_low) &
(nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) &
(nonzeroy < win_y_high) &
(nonzerox >= win_xright_low) &
(nonzerox < win_xright_high)).nonzero()[0]
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
if len(good_left_inds) > minpix:
left_x_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
right_x_current = np.int(np.mean(nonzerox[good_right_inds]))
# Concatenate the arrays of indices
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
# Extract left and right line pixel positions
leftx = nonzerox[left_lane_inds]
lefty = nonzeroy[left_lane_inds]
rightx = nonzerox[right_lane_inds]
righty = nonzeroy[right_lane_inds]
# Fit a second order polynomial to each
left_fit = np.polyfit(lefty, leftx, 2)
right_fit = np.polyfit(righty, rightx, 2)
left_a.append(left_fit[0])
left_b.append(left_fit[1])
left_c.append(left_fit[2])
right_a.append(right_fit[0])
right_b.append(right_fit[1])
right_c.append(right_fit[2])
left_fit_[0] = np.mean(left_a[-10:])
left_fit_[1] = np.mean(left_b[-10:])
left_fit_[2] = np.mean(left_c[-10:])
right_fit_[0] = np.mean(right_a[-10:])
right_fit_[1] = np.mean(right_b[-10:])
right_fit_[2] = np.mean(right_c[-10:])
# Generate x and y values for plotting
if max(righty) >= max(lefty):
limit = max(righty)
else:
limit = max(lefty)
ploty = np.linspace(0, limit - 1, limit) # righty[0] == lefty[0]
left_fitx = left_fit_[0] * ploty**2 + left_fit_[1] * ploty + left_fit_[
2]
right_fitx = right_fit_[0] * ploty**2 + right_fit_[
1] * ploty + right_fit_[2]
# plt.plot(left_fitx, ploty, right_fitx, ploty)
# plt.show()
warped_img[nonzeroy[left_lane_inds],
nonzerox[left_lane_inds]] = [255, 0, 100]
warped_img[nonzeroy[right_lane_inds],
nonzerox[right_lane_inds]] = [0, 100, 255]
color_img = np.zeros_like(warped_img)
lane_img = color_img
for a in range(len(left_fitx)):
cv2.circle(lane_img, (int(left_fitx[a]), int(ploty[a])),
5, (255, 0, 0),
thickness=-1)
for b in range(len(right_fitx)):
cv2.circle(lane_img, (int(right_fitx[b]), int(ploty[b])),
5, (0, 255, 0),
thickness=-1)
left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
right = np.array(
[np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
points = np.hstack((left, right))
cv2.fillPoly(color_img, np.int_(points), (0, 200, 255))
inv_p_wrap = cv2.warpPerspective(color_img, inv_mat,
(cv_img.shape[1], cv_img.shape[0]))
inv_lane_warp = cv2.warpPerspective(lane_img, inv_mat,
(cv_img.shape[1], cv_img.shape[0]))
inv_p_wrap = cv2.addWeighted(cv_img, 1, inv_p_wrap, 0.7, 0)
inv_p_wrap = cv2.addWeighted(cv_img, 1, inv_lane_warp, 0.5, 0)
inv_p_wrap = cv2.cvtColor(inv_p_wrap, cv2.COLOR_BGR2RGB)
img_plane_midpoint = np.dot(
inv_mat,
np.float32([(right_fitx[-1] - left_fitx[-1]) / 2 + left_fitx[-1],
ploty[-1], 1]).T)
img_plane_midpoint = np.array([
img_plane_midpoint[0] / img_plane_midpoint[2],
img_plane_midpoint[1] / img_plane_midpoint[2]
],
dtype=int)
offset = cv_img.shape[1] // 2 - img_plane_midpoint[0]
index_y_ref = height - 40
cv2.rectangle(inv_p_wrap, (400, 100), (640, 0), (255, 255, 255),
thickness=-1,
lineType=8,
shift=0)
fontFace = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(inv_p_wrap, "information box", (465, 20), fontFace, .5,
(0, 0, 0))
cv2.putText(inv_p_wrap, "pixel offset: " + str(offset) + " pxls",
(440, 50), fontFace, .5, (0, 0, 0))
cv2.line(inv_p_wrap, (img_plane_midpoint[0], index_y_ref - 35),
(img_plane_midpoint[0], index_y_ref + 35), (255, 255, 255), 3)
cv2.line(inv_p_wrap, (width // 2, index_y_ref - 5 - 35),
(width // 2, index_y_ref - 5 + 35), (0, 0, 0), 3)
cv2.line(inv_p_wrap, (0, horizon[1]), (width, horizon[1]),
(255, 255, 255), 1) # horizon line ######################
cv2.line(inv_p_wrap, (horizon[0], 0), (horizon[0], height),
(255, 255, 255), 1) # horizon line ######################
cv2.putText(inv_p_wrap, str(offset), (width // 2, 255), fontFace, .5,
(0, 0, 255))
if offset > 0:
cv2.arrowedLine(inv_p_wrap, (width // 2 + 40, 250),
(width // 2 + 90, 250), (255, 0, 0), 2, 8, 0, 0.5)
elif offset < 0:
cv2.arrowedLine(inv_p_wrap, (width // 2 - 10, 250),
(width // 2 - 60, 250), (255, 0, 0), 2, 8, 0, 0.5)
else:
cv2.arrowedLine(inv_p_wrap, (width // 2, 230), (width // 2, 180),
(255, 0, 0), 2, 8, 0, 0.5)
image_out = self.Bridge.cv2_to_imgmsg(inv_p_wrap, "bgr8")
self.pub.publish(image_out)
self.offset_pub.publish(offset)
# To Display Image
# cv2.imshow('new_img', inv_p_wrap)
# cv2.waitKey(3)
### world cordinate calculation ###
image_lane_loc = []
image_pts = []
for idx in range(len(ploty)):
llane = self.h_transform(left_fitx[idx], ploty[idx], inv_mat)
w_llane = self.mono.imageToVehicle(llane)
rlane = self.h_transform(right_fitx[idx], ploty[idx], inv_mat)
w_rlane = self.mono.imageToVehicle(rlane)
if len(image_lane_loc) is 0:
image_lane_loc = np.array([llane, rlane]).reshape(-1)
image_pts = np.array([w_llane, w_rlane]).reshape(-1)
else:
image_lane_loc = np.vstack(
(image_lane_loc, np.array([llane, rlane]).reshape(-1)))
image_pts = np.vstack(
(image_pts, np.array([w_llane, w_rlane]).reshape(-1)))
(row, col) = (image_lane_loc.shape[0], image_lane_loc.shape[1])
# plt.plot(image_lane_loc[:,0], 480-image_lane_loc[:,1], image_lane_loc[:,2],480-image_lane_loc[:,3])
# plt.show()
# plt.plot(image_pts[:,1], image_pts[:,0], image_pts[:,3], image_pts[:,2])
# plt.xlim((1,-1))
# plt.show()
ros_msg = image_pts.reshape(-1)
msg = Lanepoints()
msg.rows = int(row)
msg.cols = int(col)
msg.loc = ros_msg
self.lane_pub.publish(msg)
class acquisitionProcessor():
"""
Processes the data coming from a remote device (Duckiebot or watchtower).
"""
def __init__(self, outputDictQueue):
# Get the environment variables
self.ACQ_POSES_UPDATE_RATE = float(os.getenv('ACQ_POSES_UPDATE_RATE', 10)) #Hz
self.ACQ_ODOMETRY_UPDATE_RATE = float(os.getenv('ACQ_ODOMETRY_UPDATE_RATE', 30)) #Hz
self.ACQ_STATIONARY_ODOMETRY = bool(int(os.getenv('ACQ_STATIONARY_ODOMETRY', 0)))
self.ACQ_SOCKET_HOST = os.getenv('ACQ_SOCKET_HOST', '127.0.0.1')
self.ACQ_SOCKET_PORT = int(os.getenv('ACQ_SOCKET_PORT', 65432))
self.ACQ_DEVICE_NAME = os.getenv('ACQ_DEVICE_NAME', "watchtower10")
self.ACQ_TOPIC_RAW = os.getenv('ACQ_TOPIC_RAW', "camera_node/image/raw")
self.ACQ_TOPIC_CAMERAINFO = os.getenv('ACQ_TOPIC_CAMERAINFO', "camera_node/camera_info")
self.ACQ_TOPIC_VELOCITY_TO_POSE = os.getenv('ACQ_TOPIC_VELOCITY_TO_POSE', None)
self.ACQ_TEST_STREAM = bool(int(os.getenv('ACQ_TEST_STREAM', 1)))
self.ACQ_BEAUTIFY = bool(int(os.getenv('ACQ_BEAUTIFY', 1)))
self.ACQ_TAG_SIZE = float(os.getenv('ACQ_TAG_SIZE', 0.065))
self.ACQ_IMAGE_SCALE = float(os.getenv('ACQ_IMAGE_SCALE', 1.0))
# Initialize ROS nodes and subscribe to topics
rospy.init_node('acquisition_processor', anonymous=True, disable_signals=True)
self.subscriberRawImage = rospy.Subscriber("/"+self.ACQ_DEVICE_NAME+"/"+self.ACQ_TOPIC_RAW, CompressedImage,
self.camera_image_callback, queue_size = 1)
self.subscriberCameraInfo = rospy.Subscriber("/"+self.ACQ_DEVICE_NAME+"/"+self.ACQ_TOPIC_CAMERAINFO, CameraInfo,
self.camera_info_callback, queue_size = 1)
if self.ACQ_TOPIC_VELOCITY_TO_POSE and self.ACQ_ODOMETRY_UPDATE_RATE>0: #Only if set (probably not for watchtowers)
self.subscriberCameraInfo = rospy.Subscriber("/"+self.ACQ_DEVICE_NAME+"/"+self.ACQ_TOPIC_VELOCITY_TO_POSE, Pose2DStamped,
self.odometry_callback, queue_size = 1)
# CvBridge is neccessary for the image processing
self.bridge = CvBridge()
# Initialize atributes
self.lastCameraInfo = None
self.lastCameraImage = None
self.lastImageProcessed = False
self.previousOdometry = Pose2DStamped()
self.previousOdometry.x = 0.0
self.previousOdometry.y = 0.0
self.previousOdometry.theta = 0.0
self.timeLastPub_odometry = rospy.get_time()
self.timeLastPub_poses = rospy.get_time()
self.outputDictQueue = outputDictQueue
def update(self, quitEvent):
"""
Runs constantly and processes new data as it comes.
"""
while not quitEvent.is_set():
#check if we want odometry and if it has been sent in the last X secs
if self.ACQ_STATIONARY_ODOMETRY and self.ACQ_TOPIC_VELOCITY_TO_POSE and self.ACQ_ODOMETRY_UPDATE_RATE>0 and rospy.get_time() - self.timeLastPub_odometry >= 1.0/self.ACQ_ODOMETRY_UPDATE_RATE:
odometry = TransformStamped()
odometry.header.seq = 0
odometry.header.stamp = rospy.get_rostime()
odometry.header.frame_id = self.ACQ_DEVICE_NAME
odometry.child_frame_id = self.ACQ_DEVICE_NAME
odometry.transform.translation.x = 0
odometry.transform.translation.y = 0
odometry.transform.translation.z = 0
odometry.transform.rotation.x = 0
odometry.transform.rotation.y = 0
odometry.transform.rotation.z = 0
odometry.transform.rotation.w = 1
self.timeLastPub_odometry = rospy.get_time()
self.outputDictQueue.put(obj=pickle.dumps({"odometry": odometry}, protocol=-1),
block=True,
timeout=None)
# Check if the last data was not yet processed and if it's time to process it (in order to sustain the deisred update rate)
if rospy.get_time() - self.timeLastPub_poses >= 1.0/self.ACQ_POSES_UPDATE_RATE and not self.lastImageProcessed:
self.timeLastPub_poses = rospy.get_time()
outputDict = self.camera_image_process()
if outputDict is not None:
self.outputDictQueue.put(obj=pickle.dumps(outputDict, protocol=-1),
block=True,
timeout=None)
self.lastImageProcessed = True
def odometry_callback(self, ros_data):
"""
Callback function that is executed upon reception of new odometry data.
"""
try:
# Prepare the new ROS message for the processed odometry
odometry = TransformStamped()
odometry.header.seq = 0
odometry.header = ros_data.header
odometry.header.frame_id = self.ACQ_DEVICE_NAME
odometry.child_frame_id = self.ACQ_DEVICE_NAME
# Transform the incoming data to quaternions
transform_current = np.array([[math.cos(ros_data.theta), -1.0 * math.sin(ros_data.theta), ros_data.x],
[math.sin(ros_data.theta), math.cos(ros_data.theta), ros_data.y],
[0.0, 0.0, 1.0]])
transform_previous = np.array([[math.cos(self.previousOdometry.theta), -1.0 * math.sin(self.previousOdometry.theta), self.previousOdometry.x],
[math.sin(self.previousOdometry.theta), math.cos(self.previousOdometry.theta), self.previousOdometry.y],
[0.0, 0.0, 1.0]])
transform_previous_inv = np.linalg.inv(transform_previous)
self.previousOdometry = ros_data
transform_relative = np.matmul(transform_previous_inv, transform_current)
angle = math.atan2(transform_relative[1][0], transform_relative[0][0])
x = transform_relative[0][2]
y = transform_relative[1][2]
cy = math.cos(angle * 0.5)
sy = math.sin(angle * 0.5)
cp = 1.0
sp = 0.0
cr = 1.0
sr = 0.0
q_w = cy * cp * cr + sy * sp * sr
q_x = cy * cp * sr - sy * sp * cr
q_y = sy * cp * sr + cy * sp * cr
q_z = sy * cp * cr - cy * sp * sr
# Save the resuts to the new odometry relative pose message
odometry.transform.translation.x = x
odometry.transform.translation.y = y
odometry.transform.translation.z = 0
odometry.transform.rotation.x = q_x
odometry.transform.rotation.y = q_y
odometry.transform.rotation.z = q_z
odometry.transform.rotation.w = q_w
# Add the new data to the queue so that the server side publisher can publish them on the other ROS Master
self.outputDictQueue.put(obj=pickle.dumps({"odometry": odometry}, protocol=-1),
block=True,
timeout=None)
# Update the last odometry time
self.timeLastPub_odometry = rospy.get_time()
except Exception as e:
print Exception("Odometry data not generated, exception encountered: %s" % str(e))
pass
def camera_image_process(self):
"""
Contains the camera image processing necessary.
"""
outputDict = None
# If there is new data to process
if self.lastCameraInfo is not None and self.lastCameraImage is not None:
# Collect latest ros_data
currRawImage = self.lastCameraImage
currCameraInfo = self.lastCameraInfo
# Convert from ROS image message to numpy array
cv_image = self.bridge.compressed_imgmsg_to_cv2(currRawImage, desired_encoding="mono8")
# Scale the K matrix if the image resolution is not the same as in the calibration
currRawImage_height = cv_image.shape[0]
currRawImage_width = cv_image.shape[1]
scale_matrix = np.ones(9)
if currCameraInfo.height != currRawImage_height or currCameraInfo.width != currRawImage_width:
scale_width = float(currRawImage_width) / currCameraInfo.width
scale_height = float(currRawImage_height) / currCameraInfo.height
scale_matrix[0] *= scale_width
scale_matrix[2] *= scale_width
scale_matrix[4] *= scale_height
scale_matrix[5] *= scale_height
outputDict = dict()
# Process the image and extract the apriltags
dsp_options={"beautify": self.ACQ_BEAUTIFY, "tag_size": self.ACQ_TAG_SIZE}
dsp = deviceSideProcessor(dsp_options)
outputDict = dsp.process(cv_image, (np.array(currCameraInfo.K)*scale_matrix).reshape((3,3)), currCameraInfo.D)
# Add the time stamp and source of the input image to the output
for idx in range(len(outputDict["apriltags"])):
outputDict["apriltags"][idx]["timestamp_secs"] = currRawImage.header.stamp.secs
outputDict["apriltags"][idx]["timestamp_nsecs"] = currRawImage.header.stamp.nsecs
outputDict["apriltags"][idx]["source"] = self.ACQ_DEVICE_NAME
# Generate a diagnostic image
if self.ACQ_TEST_STREAM==1:
try:
image = np.copy(outputDict['rect_image'])
# Put the AprilTag bound boxes and numbers to the image
for tag in outputDict["apriltags"]:
for idx in range(len(tag["corners"])):
cv2.line(image, tuple(tag["corners"][idx-1, :].astype(int)), tuple(tag["corners"][idx, :].astype(int)), (0, 255, 0))
# cv2.rectangle(image, (tag["corners"][0, 0].astype(int)-10,tag["corners"][0, 1].astype(int)-10), (tag["corners"][0, 0].astype(int)+15,tag["corners"][0, 1].astype(int)+15), (0, 0, 255), cv2.FILLED)
cv2.putText(image,str(tag["tag_id"]),
org=(tag["corners"][0, 0].astype(int)+10,tag["corners"][0, 1].astype(int)+10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.4,
color=(255, 0, 0))
# Put device and timestamp to the image
cv2.putText(image,'device: '+ self.ACQ_DEVICE_NAME +', timestamp: '+str(currRawImage.header.stamp.secs)+"+"+str(currRawImage.header.stamp.nsecs),
org=(30,30),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=0.6,
color=(0, 255, 0))
# Add the original and diagnostic info to the outputDict
outputDict["test_stream_image"] = self.bridge.cv2_to_compressed_imgmsg(image, dst_format='png')
outputDict["test_stream_image"].header.stamp.secs = currRawImage.header.stamp.secs
outputDict["test_stream_image"].header.stamp.nsecs = currRawImage.header.stamp.nsecs
outputDict["test_stream_image"].header.frame_id = self.ACQ_DEVICE_NAME
outputDict["raw_image"] = self.lastCameraImage
outputDict["raw_camera_info"] = self.lastCameraInfo
outputDict["rectified_image"] = self.bridge.cv2_to_compressed_imgmsg(outputDict["rect_image"], dst_format='png')
outputDict["rectified_image"].header.stamp.secs = currRawImage.header.stamp.secs
outputDict["rectified_image"].header.stamp.nsecs = currRawImage.header.stamp.nsecs
outputDict["rectified_image"].header.frame_id = self.ACQ_DEVICE_NAME
#THIS DOESN'T WORK YET
# print(outputDict["newCameraMatrix"])
# outputDict["rectified_camera_info"] = self.lastCameraInfo
# outputDict["rectified_camera_info"].K = list(np.array(outputDict["newCameraMatrix"].flatten(order="C")))
except Exception as e:
print("Test data not generated, exception encountered: %s" % str(e))
pass
return outputDict
def camera_info_callback(self, ros_data):
"""
Callback function that is executed upon reception of new camera info data.
"""
self.lastCameraInfo = ros_data
def camera_image_callback(self, ros_data):
"""
Callback function that is executed upon reception of a new camera image.
"""
self.lastCameraImage = ros_data
self.lastImageProcessed = False
class acquisitionProcessor():
"""
Processes the data coming from a remote device (Duckiebot or watchtower).
"""
def __init__(self, logger, mode='live'):
self.mode = mode
if self.mode != 'live' and self.mode != 'postprocessing':
raise Exception(
"The argument mode should be 'live' or 'postprocessing'. Received %s instead." % self.mode)
# Get the environment variables
self.ACQ_DEVICE_NAME = os.getenv('ACQ_DEVICE_NAME', 'watchtower10')
self.ACQ_TOPIC_RAW = os.getenv(
'ACQ_TOPIC_RAW', 'camera_node/image/compressed')
self.ACQ_TOPIC_CAMERAINFO = os.getenv(
'ACQ_TOPIC_CAMERAINFO', 'camera_node/camera_info')
self.ACQ_TOPIC_VELOCITY_TO_POSE = os.getenv(
'ACQ_TOPIC_VELOCITY_TO_POSE', None)
self.ACQ_TEST_STREAM = bool(int(os.getenv('ACQ_TEST_STREAM', 1)))
self.ACQ_BEAUTIFY = bool(int(os.getenv('ACQ_BEAUTIFY', 1)))
self.ACQ_TAG_SIZE = float(os.getenv('ACQ_TAG_SIZE', 0.065))
self.ACQ_STATIONARY_ODOMETRY = bool(
int(os.getenv('ACQ_STATIONARY_ODOMETRY', 0)))
self.ACQ_APRILTAG_QUAD_DECIMATE = float(
os.getenv('ACQ_APRILTAG_QUAD_DECIMATE', 1.0))
self.ACQ_ODOMETRY_POST_VISUAL_ODOMETRY = bool(
int(os.getenv('ACQ_ODOMETRY_POST_VISUAL_ODOMETRY', 0)))
self.ACQ_ODOMETRY_POST_VISUAL_ODOMETRY_FEATURES = os.getenv(
'ACQ_ODOMETRY_POST_VISUAL_ODOMETRY_FEATURES', 'SURF')
if self.mode == 'live':
self.ACQ_POSES_UPDATE_RATE = float(
os.getenv('ACQ_POSES_UPDATE_RATE', 10)) # Hz
self.ACQ_ODOMETRY_UPDATE_RATE = float(
os.getenv('ACQ_ODOMETRY_UPDATE_RATE', 30)) # Hz
elif self.mode == 'postprocessing':
pass
# Initialize ROS nodes and subscribe to topics
if self.mode == 'live':
rospy.init_node('acquisition_processor',
anonymous=True, disable_signals=True)
self.subscriberRawImage = rospy.Subscriber('/'+self.ACQ_DEVICE_NAME+'/'+self.ACQ_TOPIC_RAW, CompressedImage,
self.camera_image_callback, queue_size=1)
self.subscriberCameraInfo = rospy.Subscriber('/'+self.ACQ_DEVICE_NAME+'/'+self.ACQ_TOPIC_CAMERAINFO, CameraInfo,
self.camera_info_callback, queue_size=1)
# Only if set (probably not for watchtowers)
if self.ACQ_TOPIC_VELOCITY_TO_POSE and self.ACQ_ODOMETRY_UPDATE_RATE > 0:
self.subscriberCameraInfo = rospy.Subscriber('/'+self.ACQ_DEVICE_NAME+'/'+self.ACQ_TOPIC_VELOCITY_TO_POSE, Pose2DStamped,
self.odometry_callback, queue_size=1)
elif self.mode == 'postprocessing':
self.bag_topics = {'raw_image': '/'+self.ACQ_DEVICE_NAME+'/'+self.ACQ_TOPIC_RAW,
'camera_info': '/'+self.ACQ_DEVICE_NAME+'/'+self.ACQ_TOPIC_CAMERAINFO}
# Only if set (probably not for watchtowers)
if self.ACQ_TOPIC_VELOCITY_TO_POSE:
self.bag_topics['odometry'] = '/'+self.ACQ_DEVICE_NAME + \
'/'+self.ACQ_TOPIC_VELOCITY_TO_POSE
# CvBridge is neccessary for the image processing
self.bridge = CvBridge()
# Initialize atributes
self.lastCameraInfo = None
self.lastCameraImage = None
self.lastImageProcessed = True
self.lastOdometry = None
self.lastOdometryProcessed = True
self.previousOdometry = {'x': 0.0, 'y': 0.0, 'theta': 0.0}
if self.mode == 'live':
self.timeLastPub_odometry = rospy.get_time()
self.timeLastPub_poses = rospy.get_time()
self.logger = logger
# Initialize the device side processor
self.dsp_options = {'beautify': self.ACQ_BEAUTIFY,
'tag_size': self.ACQ_TAG_SIZE}
self.dsp = None
self.logger.info('Acquisition processor is set up.')
def liveUpdate(self, outputDictQueue, quitEvent):
"""
Runs constantly and processes new data as it comes.
"""
assert(self.mode == 'live')
while not quitEvent.is_set():
# check if we want odometry:
if self.ACQ_TOPIC_VELOCITY_TO_POSE and self.ACQ_POSES_UPDATE_RATE > 0 and rospy.get_time() - self.timeLastPub_poses >= 1.0/self.ACQ_POSES_UPDATE_RATE:
odometry = None
# If we have a new actual message, use it
if not self.lastOdometryProcessed:
odometry = self.odometry_process(
self.lastOdometry.x, self.lastOdometry.y, self.lastOdometry.theta, self.lastOdometry.header)
self.lastOdometryProcessed = True
# Else send a dummy message if requested and if live (only if no other message has been sent for the last 0.2 secs)
elif self.ACQ_STATIONARY_ODOMETRY:
odometry = TransformStamped()
odometry.header.seq = 0
odometry.header.stamp = rospy.get_rostime()
odometry.header.frame_id = self.ACQ_DEVICE_NAME
odometry.child_frame_id = self.ACQ_DEVICE_NAME
odometry.transform.translation.x = 0
odometry.transform.translation.y = 0
odometry.transform.translation.z = 0
odometry.transform.rotation.x = 0
odometry.transform.rotation.y = 0
odometry.transform.rotation.z = 0
odometry.transform.rotation.w = 1
# Do these only if an odometry message was generated
if odometry is not None:
# Update the last odometry time
if self.mode == 'live':
self.timeLastPub_odometry = rospy.get_time()
outputDictQueue.put(obj=pickle.dumps({'odometry': odometry}, protocol=-1),
block=True,
timeout=None)
# Check if the last image data was not yet processed and if it's time to process it (in order to sustain the deisred update rate)
if rospy.get_time() - self.timeLastPub_poses >= 1.0/self.ACQ_POSES_UPDATE_RATE and not self.lastImageProcessed:
self.timeLastPub_poses = rospy.get_time()
if self.lastCameraInfo is not None and self.lastCameraImage is not None:
# Collect latest ros_data
currRawImage = self.lastCameraImage
currCameraInfo = self.lastCameraInfo
outputDict = self.camera_image_process(
currRawImage, currCameraInfo)
if outputDict is not None:
outputDictQueue.put(obj=pickle.dumps(outputDict, protocol=-1),
block=True,
timeout=None)
self.lastImageProcessed = True
def postprocess(self, outputDictQueue, bag, n_threads=8):
"""
Processes the data from a ROS bag.
IMPORTANT: This will load all the bag contents in RAM! Make sure that
the bag size is not too big and that you have sufficient RAM memory.
"""
assert(self.mode == 'postprocessing')
self.logger.info('Postprocessing started')
# Setup the ProcessingPool. We use pathos as it uses dill instead of pickle which packeges more classes
p = ProcessingPool(n_threads)
self.logger.info('Bag loaded')
# We process each topic separately.
# [CAMERA INFO] The images can be processed in parallel. We assume the camera info does not change so we simply take the first one
self.logger.info('Getting the camera info')
for (topic, msg, t) in bag.read_messages(topics=self.bag_topics['camera_info']):
cameraInfo = self.debagify_msg(msg)
break
self.logger.info('Camera info obtained')
# [APRIL TAGS]
def full_process(msg):
return self.camera_image_process(msg, cameraInfo)
self.logger.info('Loading the camera images')
msgs_to_process = [self.debagify_msg(msg) for (
topic, msg, t) in bag.read_messages(topics=self.bag_topics['raw_image'])]
self.logger.info('Processing the camera images')
results = p.map(full_process, msgs_to_process)
for result in results:
if result is not None:
outputDictQueue.put(obj=pickle.dumps(result, protocol=-1),
block=True,
timeout=None)
self.logger.info('Finished processing the camera images')
# [ODOMETRY] Odometry needs to be processed sequentially as it has to subtract the poses of subsequent messages
if self.ACQ_ODOMETRY_POST_VISUAL_ODOMETRY: # If visual odometry was requested
self.logger.info(
'Odometry processing starting. Using Visual Odometry (VO). This will be slow.')
# We reuse the rectified images and the new camera matrix from results
K = results[0]['new_camera_matrix'].reshape((3, 3))
# Setting up the visual odometry
vo = VisualOdometry(
K, self.ACQ_ODOMETRY_POST_VISUAL_ODOMETRY_FEATURES, pitch_adjust=np.deg2rad(10.0))
clahe = cv2.createCLAHE(clipLimit=5.0)
# Iterating through the images one by one
for img_id, res in enumerate(results):
img = res['rect_image']
img = clahe.apply(img)
if vo.update(img, img_id):
# The scaling is rather arbitrary, it seems that VO gives centimeters, but in general the scaling is fishy...
odometry = TransformStamped()
odometry.header.seq = 0
odometry.header = res['header']
odometry.header.frame_id = self.ACQ_DEVICE_NAME
odometry.child_frame_id = self.ACQ_DEVICE_NAME
cy = math.cos(vo.relative_pose_theta * 0.5)
sy = math.sin(vo.relative_pose_theta * 0.5)
cp = 1.0
sp = 0.0
cr = 1.0
sr = 0.0
q_w = cy * cp * cr + sy * sp * sr
q_x = cy * cp * sr - sy * sp * cr
q_y = sy * cp * sr + cy * sp * cr
q_z = sy * cp * cr - cy * sp * sr
# Save the resuts to the new odometry relative pose message
odometry.transform.translation.x = vo.relative_pose_x
odometry.transform.translation.y = vo.relative_pose_y
odometry.transform.translation.z = 0
odometry.transform.rotation.x = q_x
odometry.transform.rotation.y = q_y
odometry.transform.rotation.z = q_z
odometry.transform.rotation.w = q_w
outputDictQueue.put(obj=pickle.dumps({'odometry': odometry}, protocol=-1),
block=True,
timeout=None)
if img_id % 100 == 0:
self.logger.info('VO: %d/%d frames processed.' %
(img_id+1, len(results)))
self.logger.info('Odometry processing finished')
# If VO was not requested and only if set (probably not for watchtowers)
elif self.ACQ_TOPIC_VELOCITY_TO_POSE:
self.logger.info(
'Odometry processing starting. Using topic %s' % self.ACQ_TOPIC_VELOCITY_TO_POSE)
for (topic, msg, t) in bag.read_messages(topics=self.bag_topics['odometry']):
odometry = self.odometry_process(
msg.x, msg.y, msg.theta, msg.header)
outputDictQueue.put(obj=pickle.dumps({'odometry': odometry}, protocol=-1),
block=True,
timeout=None)
self.logger.info('Odometry processing finished')
bag.close()
def odometry_process(self, x, y, theta, header):
"""
Callback function that is executed upon reception of new odometry data.
"""
# Prepare the new ROS message for the processed odometry
odometry = TransformStamped()
odometry.header.seq = 0
odometry.header = header
odometry.header.frame_id = self.ACQ_DEVICE_NAME
odometry.child_frame_id = self.ACQ_DEVICE_NAME
# Transform the incoming data to quaternions
transform_current = np.array([[math.cos(theta), -1.0 * math.sin(theta), x],
[math.sin(theta), math.cos(theta), y],
[0.0, 0.0, 1.0]])
transform_previous = np.array([[math.cos(self.previousOdometry["theta"]), -1.0 * math.sin(self.previousOdometry["theta"]), self.previousOdometry["x"]],
[math.sin(self.previousOdometry["theta"]), math.cos(
self.previousOdometry["theta"]), self.previousOdometry["y"]],
[0.0, 0.0, 1.0]])
transform_previous_inv = np.linalg.inv(transform_previous)
self.previousOdometry = {'x': x, 'y': y, 'theta': theta}
transform_relative = np.matmul(
transform_previous_inv, transform_current)
angle = math.atan2(transform_relative[1][0], transform_relative[0][0])
x = transform_relative[0][2]
y = transform_relative[1][2]
cy = math.cos(angle * 0.5)
sy = math.sin(angle * 0.5)
cp = 1.0
sp = 0.0
cr = 1.0
sr = 0.0
q_w = cy * cp * cr + sy * sp * sr
q_x = cy * cp * sr - sy * sp * cr
q_y = sy * cp * sr + cy * sp * cr
q_z = sy * cp * cr - cy * sp * sr
# Save the resuts to the new odometry relative pose message
odometry.transform.translation.x = x
odometry.transform.translation.y = y
odometry.transform.translation.z = 0
odometry.transform.rotation.x = q_x
odometry.transform.rotation.y = q_y
odometry.transform.rotation.z = q_z
odometry.transform.rotation.w = q_w
return odometry
def camera_image_process(self, currRawImage, currCameraInfo):
"""
Contains the necessary camera image processing.
"""
outputDict = None
# Use the right dsp, depending on the mode
if self.mode == 'live':
if self.dsp == None:
self.dsp = deviceSideProcessor(self.dsp_options, self.logger)
dsp = self.dsp
if self.mode == 'postprocessing':
global dsp
if dsp == None:
dsp = deviceSideProcessor(self.dsp_options, self.logger)
assert(dsp is not None)
# Convert from ROS image message to numpy array
cv_image = self.bridge.compressed_imgmsg_to_cv2(
currRawImage, desired_encoding='mono8')
# Scale the K matrix if the image resolution is not the same as in the calibration
currRawImage_height = cv_image.shape[0]
currRawImage_width = cv_image.shape[1]
scale_matrix = np.ones(9)
if currCameraInfo.height != currRawImage_height or currCameraInfo.width != currRawImage_width:
scale_width = float(currRawImage_width) / currCameraInfo.width
scale_height = float(currRawImage_height) / currCameraInfo.height
scale_matrix[0] *= scale_width
scale_matrix[2] *= scale_width
scale_matrix[4] *= scale_height
scale_matrix[5] *= scale_height
outputDict = dict()
# Process the image and extract the apriltags
outputDict = dsp.process(cv_image, (np.array(
currCameraInfo.K)*scale_matrix).reshape((3, 3)), currCameraInfo.D)
outputDict['header'] = currRawImage.header
# Add the time stamp and source of the input image to the output
for idx in range(len(outputDict['apriltags'])):
outputDict['apriltags'][idx]['timestamp_secs'] = currRawImage.header.stamp.secs
outputDict['apriltags'][idx]['timestamp_nsecs'] = currRawImage.header.stamp.nsecs
outputDict['apriltags'][idx]['source'] = self.ACQ_DEVICE_NAME
# Generate a diagnostic image
if self.ACQ_TEST_STREAM == 1:
image = np.copy(outputDict['rect_image'])
# Put the AprilTag bound boxes and numbers to the image
for tag in outputDict['apriltags']:
for idx in range(len(tag['corners'])):
cv2.line(image, tuple(tag['corners'][idx-1, :].astype(int)),
tuple(tag['corners'][idx, :].astype(int)), (0, 255, 0))
# cv2.rectangle(image, (tag['corners'][0, 0].astype(int)-10,tag['corners'][0, 1].astype(int)-10), (tag['corners'][0, 0].astype(int)+15,tag['corners'][0, 1].astype(int)+15), (0, 0, 255), cv2.FILLED)
cv2.putText(image, str(tag['tag_id']),
org=(tag['corners'][0, 0].astype(int)+10,
tag['corners'][0, 1].astype(int)+10),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1.0,
thickness=2,
color=(255, 0, 0))
# Put device and timestamp to the image
cv2.putText(image, 'device: ' + self.ACQ_DEVICE_NAME + ', timestamp: '+str(currRawImage.header.stamp.secs)+'+'+str(currRawImage.header.stamp.nsecs),
org=(30, 30),
fontFace=cv2.FONT_HERSHEY_SIMPLEX,
fontScale=1.0,
thickness=2,
color=(255, 0, 0))
# Add the original and diagnostic info to the outputDict
outputDict['test_stream_image'] = self.bridge.cv2_to_compressed_imgmsg(
image, dst_format='png')
outputDict['test_stream_image'].header.stamp.secs = currRawImage.header.stamp.secs
outputDict['test_stream_image'].header.stamp.nsecs = currRawImage.header.stamp.nsecs
outputDict['test_stream_image'].header.frame_id = self.ACQ_DEVICE_NAME
outputDict['raw_image'] = currRawImage
outputDict['raw_camera_info'] = currCameraInfo
outputDict['rectified_image'] = self.bridge.cv2_to_compressed_imgmsg(
outputDict['rect_image'], dst_format='png')
outputDict['rectified_image'].header.stamp.secs = currRawImage.header.stamp.secs
outputDict['rectified_image'].header.stamp.nsecs = currRawImage.header.stamp.nsecs
outputDict['rectified_image'].header.frame_id = self.ACQ_DEVICE_NAME
return outputDict
def camera_info_callback(self, ros_data):
"""
Callback function that is executed upon reception of new camera info data.
"""
self.lastCameraInfo = ros_data
def camera_image_callback(self, ros_data):
"""
Callback function that is executed upon reception of a new camera image.
"""
self.lastCameraImage = ros_data
self.lastImageProcessed = False
def odometry_callback(self, ros_data):
"""
Callback function that is executed upon reception of a new odometry message.
"""
self.lastOdometry = ros_data
self.lastOdometryProcessed = False
def debagify_msg(self, msg):
"""
The messages read from the rosbags do not have the correct classes so we need to reinitialize themself.
"""
if 'Header' in str(type(msg)):
newMsg = Header()
newMsg.seq = msg.seq
newMsg.stamp.secs = msg.stamp.secs
newMsg.stamp.nsecs = msg.stamp.nsecs
newMsg.frame_id = msg.frame_id
return newMsg
if 'RegionOfInterest' in str(type(msg)):
newMsg = RegionOfInterest()
newMsg.x_offset = msg.x_offset
newMsg.y_offset = msg.y_offset
newMsg.height = msg.height
newMsg.width = msg.width
newMsg.do_rectify = msg.do_rectify
return newMsg
if '_sensor_msgs__CompressedImage' in str(type(msg)):
im = self.bridge.compressed_imgmsg_to_cv2(msg).copy()
newMsg = self.bridge.cv2_to_compressed_imgmsg(im)
newMsg.header = self.debagify_msg(msg.header)
return newMsg
if '_sensor_msgs__CameraInfo' in str(type(msg)):
newMsg = CameraInfo()
newMsg.header = self.debagify_msg(msg.header)
newMsg.height = msg.height
newMsg.width = msg.width
newMsg.distortion_model = msg.distortion_model
newMsg.D = msg.D
newMsg.P = msg.P
newMsg.R = msg.R
newMsg.K = msg.K
newMsg.binning_x = msg.binning_x
newMsg.binning_y = msg.binning_y
newMsg.roi = self.debagify_msg(msg.roi)
return newMsg
raise Exception("Message type %s could not be debagified" %
str(type(msg)))
class ARNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,
node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(
rospack.get_path('augmented_reality_apriltag') +
'/src/models/duckie.obj')
# bridge between opencv and ros
self.bridge = CvBridge()
# construct subscriber for images
self.camera_sub = rospy.Subscriber('camera_node/image/compressed',
CompressedImage, self.callback)
# construct publisher for AR images
self.pub = rospy.Publisher('~augemented_image/compressed',
CompressedImage,
queue_size=10)
# april-tag detector
self.at_detector = Detector(searchpath=['apriltags'],
families='tag36h11',
nthreads=4,
quad_decimate=2.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# get camera calibration parameters (homography, camera matrix, distortion parameters)
self.intrinsics_file = '/data/config/calibrations/camera_intrinsic/' + \
rospy.get_namespace().strip("/") + ".yaml"
rospy.loginfo('Reading camera intrinsics from {}'.format(
self.intrinsics_file))
intrinsics = self.readYamlFile(self.intrinsics_file)
self.h = intrinsics['image_height']
self.w = intrinsics['image_width']
self.camera_mat = np.array(
intrinsics['camera_matrix']['data']).reshape(3, 3)
# Precompute some matricies
self.camera_params = (self.camera_mat[0, 0], self.camera_mat[1, 1],
self.camera_mat[0, 2], self.camera_mat[1, 2])
self.inv_camera_mat = np.linalg.inv(self.camera_mat)
self.mat_3Dto2D = np.concatenate((np.identity(3), np.zeros((3, 1))),
axis=1)
self.T = np.zeros((4, 4))
self.T[-1, -1] = 1.0
def callback(self, data):
img = self.readImage(data)
tags = self.at_detector.detect(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY),
estimate_tag_pose=True,
camera_params=self.camera_params,
tag_size=TAG_SIZE)
for tag in tags:
H_tag2img = tag.homography
projection_mat = self.projection_matrix(H_tag2img)
self.renderer.render(img, projection_mat)
msg = self.bridge.cv2_to_compressed_imgmsg(img, dst_format='jpeg')
self.pub.publish(msg)
def projection_matrix(self, homography):
"""
Write here the compuatation for the projection matrix, namely the matrix
that maps the camera reference frame to the AprilTag reference frame.
"""
T_plane = self.inv_camera_mat @ homography
T_plane = T_plane / np.linalg.norm(
T_plane[:, 0]
) # estimate scale using rotation matrix basis constraint
r1 = T_plane[:, 0]
r2 = T_plane[:, 1]
t = T_plane[:, 2]
# Make sure r1 and r2 form an orthogonal basis then generate r3
r2 = (r2 - np.dot(r2, r1) * r1)
r2 = r2 / np.linalg.norm(r2)
r3 = np.cross(r1, r2)
self.T[:3, :] = np.column_stack((r1, r2, r3, t))
return self.camera_mat @ self.mat_3Dto2D @ self.T
def readImage(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
def readYamlFile(self, fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s" %
(fname, exc),
type='fatal')
rospy.signal_shutdown()
return
def onShutdown(self):
super(ARNode, self).onShutdown()
class object_tracker:
def __init__(self):
self.bridge = CvBridge()
#self.image_sub_camera=rospy.Subscriber("/zed2/zed_node/left/image_rect_color/compressed",CompressedImage,self.callback_cam)
#self.image_sub_camera_raw=rospy.Subscriber("/zed2/zed_node/left/image_rect_color",Image,self.callback_cam_raw)
self.image_sub_depth = rospy.Subscriber(
"/zed2/zed_node/depth/depth_registered/compressed",
CompressedImage, self.callback_depth)
self.image_sub_depth_raw = rospy.Subscriber(
"/zed2/zed_node/depth/depth_registered", Image,
self.callback_depth_raw)
def callback_depth_raw(self, data):
try:
self.cv_image_depth_raw = self.bridge.imgmsg_to_cv2(data, "mono16")
#print(self.cv_image_depth_raw)
except CvBridgeError as e:
print(e)
def callback_cam_raw(self, data):
Data = data
#print(data.data)
def callback_cam(self, data):
C_array = np.fromstring(data.data, np.uint8)
self.cv_image_cam = cv2.imdecode(C_array, cv2.COLOR_BGR2RGB)
#print("Cam")
#cv2.imshow("Image_window", self.cv_image_cam)
#cv2.waitKey(3)
def callback_depth(self, data):
Data = data.data
#print(Data)
"""
depth_fmt, compr_type = data.format.split(';')
depth_fmt = depth_fmt.strip()
compr_type = compr_type.strip()
depth_header_size = 12
raw_data = data.data[depth_header_size:]
depth_img_raw = np.fromstring(raw_data, np.uint8)
"""
#print(len(depth_img_raw))
#depth_img_raw = cv2.imdecode(np.fromstring(raw_data, np.uint8), cv2.IMREAD_UNCHANGED)
#cv2.imshow("Depth",depth_img_raw)
#cv2.waitKey(1)
#print(data.data)
#D_array=np.fromstring(data.data,np.uint16)
#print(Data)
try:
self.cv_image_depth = self.bridge.compressed_imgmsg_to_cv2(
data, "rgb16")
cv2.imshow("Image_window", self.cv_image_depth)
cv2.waitKey(3)
print(self.cv_image_depth.shape)
print(self.cv_image_depth[1])
except CvBridgeError as e:
print(e)
#print(self.cv_image_depth)
"""
class CameraFocus(Node):
def __init__(self):
super().__init__('camera_focus')
self.zoom = 50
self.bruit = 0.4
self.pos = 0
self.Init = True
self.Start = False
self.Move = False
self.Points = []
self.bridge = CvBridge()
self.img = {
'left_eye': CompressedImage(),
'right_eye': CompressedImage(),
}
# self.camera_subscriber_left = self.create_subscription(
# CompressedImage, 'left_image',
# self.listener_callback_left,
# 10)
# self.camera_subscriber_left
self.camera_subscriber_right = self.create_subscription(
CompressedImage, 'right_image', self.listener_callback_right, 10)
self.camera_subscriber_right
self.cli = self.create_client(SetCameraFocusZoom,
'set_camera_focus_zoom')
while not self.cli.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...')
self.req = SetCameraFocusZoom.Request()
self.set_focus_2_cameras_client = self.create_client(
Set2CamerasFocus, 'set_2_cameras_focus')
while not self.set_focus_2_cameras_client.wait_for_service(
timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...')
self.req_focus_2_cam = Set2CamerasFocus.Request()
self.get_zoom_focus_client = self.create_client(
GetCameraFocusZoom, 'get_camera_focus_zoom')
while not self.get_zoom_focus_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...')
self.req_zoom_focus = GetCameraFocusZoom.Request()
self.keyboard_listener = keyboard.Listener(on_press=self.on_press)
self.keyboard_listener.start()
# self.t = threading.Thread(target = self.asserv,args = ('right_eye', 'left_image'), daemon = True)
self.t2 = threading.Thread(target=self.asserv,
args=('left_eye', 'right_image'),
daemon=True)
# self.t.start()
self.t2.start()
# def listener_callback_left(self, msg):
# # print ("in listener callback")
# self.img['left_image'] = self.bridge.compressed_imgmsg_to_cv2(msg)
# # cv.imshow("video",self.img)
# # print(type(self.img))
# # print(self.img)
def listener_callback_right(self, msg):
# print ("in listener callback")
self.img['right_image'] = msg
def send_request(self, name, zoom, focus):
self.req.name = name
self.req.zoom = zoom
self.req.focus = focus
self.future = self.cli.call_async(self.req)
def send_request_focus(self, left_focus, right_focus):
self.req_focus_2_cam.left_focus = left_focus
self.req_focus_2_cam.right_focus = right_focus
self.future = self.set_focus_2_cameras_client.call_async(
self.req_focus_2_cam)
def asserv(self, eye, im):
res_max = 0 # meilleur résultat de canny obtenu
p_max = 0 # posistion du focus associée à res_max
pos_min = 0 # position minimale accessible du focus
pos_max = 0 # position maximale accessible du focus
borne_inf = 0 # borne haute de tolerance du bruit
borne_sup = 0 # borne basse de tolerance du bruit
pas = 1 # pas d'avance
canny = [] # liste de stockage des résultats donnés par canny
poses = [] # liste de stockage des positions associées à canny[]
seuil = 0.5 # seuil d'interet, en dessous de cette valeur il ne peut pas y avoir de pic
k = -1 # variable de numérotation des images
j = 0 # variable de numérotation des diagrammes
sem = 0
First = True # Flag à True s'il stagit de la première itération
Stop = 0 # 0 = pas stop, 1 = 1er passage en stop, 2 = plus de 1 passage en stop
time.sleep(1)
img = self.bridge.compressed_imgmsg_to_cv2(self.img[im])
img_prec_bw = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
img_prec_bw = cv.GaussianBlur(img_prec_bw, (11, 11), 0)
while (1):
# self.req_zoom_focus.name = 'left_eye'
# self.future_zoom_focus = self.get_zoom_focus_client.call_async(self.req_zoom_focus)
# time.sleep(0.1)
# self.zoom = self.future_zoom_focus.result().zoom
# f = self.future_zoom_focus.result().focus
# print(self.zoom)
img = self.bridge.compressed_imgmsg_to_cv2(self.img[im])
if len(self.Points) == 2:
roi = img[self.Points[0][1]:self.Points[1][1],
self.Points[0][0]:self.Points[1][0]]
imgBW = cv.cvtColor(roi, cv.COLOR_BGR2GRAY)
else:
imgBW = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
if self.Start is True:
# # tp1 = time.time()
# # img_bw = cv.cvtColor(img[im], cv.COLOR_BGR2GRAY)
# t0 = time.time()
diff = cv.subtract(img_prec_bw, imgBW)
ret, diff = cv.threshold(diff, 20, 255, cv.THRESH_BINARY)
img_prec_bw = imgBW
sum = cv.integral(
diff
) # calcul la somme cumulée de touts les pixel dans l'image
move = sum[-1][-1] / (diff.shape[0] * diff.shape[1])
# t1 = time.time()
# print(t1 - t0)
# print(move)
if move >= 10 and sem < 3:
sem += 1
elif move < 10 and sem > -1:
sem -= 1
if sem >= 0:
self.Move = True
else:
self.Move = False
if self.Move is False:
res = Canny_Sharpness_function(imgBW)
canny.append(res)
poses.append(self.pos)
# print(eye+"_res = "+str(res))
# print(eye+"_res max = "+str(res_max))
if self.zoom < 100:
self.bruit = 5
if self.Init is True:
self.Init = False
First = True
Stop = 0
pos_min, pos_max = set_poses(self.zoom)
self.pos = pos_min
res_max = 0
pas = 1
# self.send_request(eye,self.zoom,self.pos) # déplacement des moteurs aux positions de départs
# time.sleep(1)
# self.test_response()
elif Stop == 0:
# print (eye+"_pos = " + str(self.pos))
# print("stop = 0")
if res > res_max:
# print("res > res_max")
res_max = res
p_max = self.pos
if First is True:
# print("First = True")
j += 1
canny = [
] # liste de stockage des résultats donnés par canny
poses = []
tp1 = time.time()
First = False
borne_inf = res - self.bruit
borne_sup = res + self.bruit
# print ("b_min = "+ str(borne_inf)+"b_max = "+str(borne_sup))
self.pos = move_to(pos_min, pos_max, self.pos, pas)
elif res < borne_inf:
print("res < borne_inf, p_max = " + str(p_max))
self.pos = p_max
# res_max = res
Stop = 1
# self.send_request('left_eye',self.zoom,self.pos-20)
# self.send_request('right_eye',self.zoom,self.pos-20)
self.send_request_focus(self.pos - 20,
self.pos - 20)
time.sleep(0.4)
self.send_request_focus(self.pos, self.pos)
time.sleep(0.4)
# time.sleep(1)
self.test_response()
# self.test_response()
tp2 = time.time()
print("!!!!!!!!!!!!time = " + str(tp2 - tp1))
elif res <= seuil:
pas = 10
self.pos = move_to(pos_min, pos_max, self.pos, pas)
elif res > borne_sup:
# print ("borne dépassée")
borne_inf = res - self.bruit
borne_sup = res + self.bruit
# print ("b_min = "+ str(borne_inf)+"b_max = "+str(borne_sup))
pas = 1
self.pos = move_to(pos_min, pos_max, self.pos, pas)
else: # borne_inf<= res <=borne_sup:
# print ("dans les bornes")
if pas == 1:
pas = 5
# elif pas == 5:
# pas = 10
self.pos = move_to(pos_min, pos_max, self.pos, pas)
elif Stop == 1:
# print ("stop = 1")
Stop = 2
# if res > res_max :
res_max = res
elif (res_max < 2 and
(res < 0.6 * res_max or res > 1.5 * res_max)) or (
res_max >= 2 and
(res < 0.8 * res_max or res > 2 * res_max)):
# print("Stop = 2 et image flou ")
# print ("res = "+str(res)+", res_max = " + str(res_max))
Stop = 0
First = True
self.pos = pos_min
res_max = 0
pas = 1
print(self.pos)
self.send_request_focus(self.pos, self.pos)
print("request sent")
# self.send_request('left_eye',self.zoom,self.pos)
# print("left request sent")
# self.send_request('right_eye',self.zoom,self.pos)
# print("right request sent")
if self.pos == pos_min:
time.sleep(1)
if pas == 10:
time.sleep(0.15)
else:
time.sleep(0.1)
# if self.pos == pos_min:
# time.sleep(1)
# if pas == 10:
# time.sleep(1)
# else :
# time.sleep(1)
self.test_response()
# self.test_response()
else:
time.sleep(0.03)
plt.figure(j)
plt.plot(poses, canny)
plt.grid()
plt.title(
"résultats traitement d'image par canny en fonction du focus")
plt.xlabel("pas du focus de la lentille")
plt.ylabel("resultat de la fonction de contraste")
plt.savefig("src/reachy_focus/datas/canny_" + str(j) + ".png")
def test_response(self):
while (rclpy.ok()):
if self.future.done():
try:
response = self.future.result()
except Exception as e:
self.get_logger().info('Service call failed %r' % (e, ))
# else:
# self.get_logger().info(
# f'Result {response.success}')
break
def on_press(
self, key
): #callback function pynput appelée à l'activation d'une touche
if str(key) == "Key.up": #augmentation du zoom de 1 pas
if self.zoom + 1 <= 600:
self.zoom += 1
self.Init = True
self.Init = True
if str(key) == "Key.down": #diminution du zoom de 1 pas
if self.zoom - 1 > 0:
self.zoom -= 1
self.Init = True
self.Init = True
if str(key) == "'z'": # modificarion du zoom
user_input = input('zoom: ')
err = 1
while (err == 1):
try:
self.zoom = int(user_input)
if 0 <= self.zoom <= 600:
err = 0
self.Init = True
self.Init = True
else:
user_input = input(
"le zoom doit être comprs entre 0 et 600, recommencez: "
)
except:
user_input = input(
"le zoom saisie n'est pas un int, recommencez: ")
if str(
key
) == "'c'": # clear all, ne garde pas en mémoire la dernière ROI selectionnée
print("clear all")
self.Init = True
self.Init = True
if self.Start is False:
self.Start = True
self.Points = []
if str(
key
) == "'r'": # restart, relance la recherche en concervant la ROI selectionnée
print("restart the sequence")
self.Init = True
self.Init = True
if self.Start is False:
self.Start = True
# if str(key) == "'i'": # choix d'une zone d'interet
# print ("selectonnez une zonne d'intérêt ")
# Start = False
# Init = True
if str(key) == "'s'": # start/stop
if self.Start is True:
self.Start = False
print("Stop")
else:
self.Start = True
print("Start")
if str(key) == "'b'":
self.Start = False
user_input = input('bruit: ')
err = 1
while (err == 1):
try:
self.bruit = float(user_input)
break
except:
user_input = input(
"le bruit saisie n'est pas un float, recommencez: ")
self.Init = True
self.Init = True
self.Start = True
class RGBDExporter:
def __init__(self):
parser = argparse.ArgumentParser()
parser.add_argument("bag_file", type=str, help="path to rosbag file")
parser.add_argument("export_dir",
type=str,
help="path to export folder")
parser.add_argument("--topic_rgb",
type=str,
default="/camera/rgb/image_rect_color/compressed",
help="colour topic (CompressedImage)")
parser.add_argument(
"--topic_depth",
type=str,
default="/camera/depth/image_rect_raw/compressedDepth",
help="depth topic (CompressedImage)")
parser.add_argument("--topic_camera_info",
type=str,
default="/camera/rgb/camera_info",
help="camera info topic (CameraInfo)")
parser.add_argument("--topic_joints",
type=str,
default="/joint_states",
help="joint state topic (JointState)")
parser.add_argument("-f",
"--force",
action="store_true",
help="overwrite old data")
parser.add_argument("-b",
"--reference_frame",
type=str,
default="base",
help="parent frame of camera pose")
args = parser.parse_args()
# input/output paths
bag_file_path = args.bag_file
self.export_path = args.export_dir
if os.path.exists(self.export_path) and not args.force:
raise UserWarning("path " + self.export_path + " already exists!")
# image topics with CompressedImage
self.topic_rgb = args.topic_rgb
self.topic_depth = args.topic_depth
# camera intrinsics with CameraInfo
self.topic_ci = args.topic_camera_info
# topic with JointState
self.topic_joints = args.topic_joints
self.topics_tf = ["/tf", "/tf_static"]
self.topics = [
self.topic_rgb, self.topic_depth, self.topic_ci, self.topic_joints
]
self.ref_frame = args.reference_frame
bag_file_path = os.path.expanduser(bag_file_path)
print("reading:", bag_file_path)
self.bag = rosbag.Bag(bag_file_path, mode='r')
print("duration:",
self.bag.get_end_time() - self.bag.get_start_time(), "s")
self.export_path = os.path.expanduser(self.export_path)
self.path_colour = os.path.join(self.export_path, "colour")
self.path_depth = os.path.join(self.export_path, "depth")
self.cvbridge = CvBridge()
def export(self):
print("exporting to: " + self.export_path)
# create export directories
for dir_path in [self.path_colour, self.path_depth]:
if not os.path.exists(dir_path):
os.makedirs(dir_path)
ref_times = []
# fill the tf buffer
# maximum duration to cache all transformations
# https://github.com/ros/geometry2/issues/356
time_max = genpy.Duration(secs=pow(2, 31) - 1)
tf_buffer = tf2_ros.Buffer(cache_time=time_max, debug=False)
for topic, msg, t in self.bag.read_messages(topics=self.topics_tf):
for transf in msg.transforms:
tf_buffer.set_transform(transf, "exporter")
# get timestamps for all messages to select reference topic with smallest amount of messages
# get available joint names and their oldest value, e.g. we are looking into the future and assume
# that the first seen joint value reflects the state of the joint before this time
topic_times = dict()
for t in self.topics:
topic_times[t] = []
full_jnt_values = dict(
) # store first (oldest) joint values of complete set
for topic, msg, t in self.bag.read_messages(topics=self.topics):
# get set of joints
if topic == self.topic_joints:
for ijoint in range(len(msg.name)):
if msg.name[ijoint] not in full_jnt_values:
full_jnt_values[
msg.name[ijoint]] = msg.position[ijoint]
topic_times[topic].append(msg.header.stamp)
if len(topic_times[self.topic_joints]) == 0:
print("Ignoring joint topic.")
else:
print("joints:", full_jnt_values.keys())
if len(topic_times[self.topic_rgb]) == 0:
print("NO colour images. Check that topic '" + self.topic_rgb +
"' is present in bag file!")
if len(topic_times[self.topic_depth]) == 0:
print("NO depth images. Check that topic '" + self.topic_depth +
"' is present in bag file!")
# remove topics with no messages
[
topic_times.pop(top, None) for top in list(topic_times.keys())
if len(topic_times[top]) == 0
]
if not topic_times:
bag_topics = self.bag.get_type_and_topic_info().topics
print("Found no messages on any of the given topics.")
print("Valid topics are:", bag_topics.keys())
print("Given topics are:", self.topics)
full_joint_list_sorted = sorted(full_jnt_values.keys())
joint_states = []
camera_poses = []
ref_topic = self.topic_rgb
# sample and hold synchronisation
sync_msg = dict()
for top in topic_times.keys():
sync_msg[top] = None
has_all_msg = False
camera_info = None
for topic, msg, t in self.bag.read_messages(topics=[self.topic_ci]):
# export a single camera info message
camera_info = msg
cp = {
'cx': msg.K[2],
'cy': msg.K[5],
'fu': msg.K[0],
'fv': msg.K[4],
'width': msg.width,
'height': msg.height
}
with open(os.path.join(self.export_path, "camera_parameters.json"),
'w') as f:
json.dump(cp,
f,
indent=4,
separators=(',', ': '),
sort_keys=True)
break
if camera_info is None:
raise Exception("No CameraInfo message received!")
for topic, msg, t in self.bag.read_messages(topics=topic_times.keys()):
if topic == self.topic_joints:
# merge all received joints
for ijoint in range(len(msg.name)):
full_jnt_values[msg.name[ijoint]] = msg.position[ijoint]
sync_msg[topic] = full_jnt_values
else:
# keep the newest message
sync_msg[topic] = msg
# export at occurrence of reference message and if all remaining messages have been received
# e.g. we export the reference message and the newest messages of other topics (sample and hold)
if topic == ref_topic and (has_all_msg or all(
[v is not None for v in sync_msg.values()])):
# faster evaluation of previous statement
has_all_msg = True
# export
ref_time = msg.header.stamp
ref_times.append(ref_time.to_nsec())
# get transformations
try:
camera_pose = tf_buffer.lookup_transform(
target_frame=self.ref_frame,
source_frame=camera_info.header.frame_id,
time=ref_time)
p = camera_pose.transform.translation
q = camera_pose.transform.rotation
camera_poses.append([p.x, p.y, p.z, q.w, q.x, q.y, q.z])
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
pass
for sync_topic in sync_msg.keys():
if sync_topic == self.topic_joints:
# export full joints, sorted by joint name
jvalues = []
for jname in full_joint_list_sorted:
jvalues.append(sync_msg[sync_topic][jname])
joint_states.append(jvalues)
elif sync_topic == self.topic_rgb:
# export RGB
if msg._type == Image._type:
if sync_msg[sync_topic].encoding[:6] == "bayer_":
desired_encoding = "bgr8"
else:
desired_encoding = "passthrough"
colour_img = self.cvbridge.imgmsg_to_cv2(
sync_msg[sync_topic],
desired_encoding=desired_encoding)
elif msg._type == CompressedImage._type:
colour_img = self.cvbridge.compressed_imgmsg_to_cv2(
sync_msg[sync_topic])
else:
print("unsupported:", msg._type)
cv2.imwrite(
os.path.join(self.path_colour,
"colour_" + str(ref_time) + ".png"),
colour_img)
elif sync_topic == self.topic_depth and msg._type == CompressedImage._type:
# export depth
depth_fmt, compr_type = sync_msg[
sync_topic].format.split(';')
# remove white space
depth_fmt = depth_fmt.strip()
compr_type = compr_type.strip()
if compr_type == "compressedDepth":
# remove header from raw data
# C header definition at:
# /opt/ros/indigo/include/compressed_depth_image_transport/compression_common.h
# enum compressionFormat {
# UNDEFINED = -1, INV_DEPTH
# };
# struct ConfigHeader {
# compressionFormat format;
# float depthParam[2];
# };
# header size = enum (4 byte) + float[2] (2 x 4 byte) = 12 byte
# enum size may vary and needs to be adapted if decoding fails
depth_header_size = 12
raw_data = sync_msg[sync_topic].data[
depth_header_size:]
depth_img_raw = cv2.imdecode(
np.frombuffer(raw_data, np.uint8),
cv2.IMREAD_UNCHANGED)
if depth_img_raw is None:
# probably wrong header size
raise Exception(
"Could not decode compressed depth image."
"You may need to change 'depth_header_size'!"
)
if depth_fmt == "16UC1":
# write raw image data
depth_img = depth_img_raw
elif depth_fmt == "32FC1":
raw_header = sync_msg[
sync_topic].data[:depth_header_size]
# header: int, float, float
[compfmt, depthQuantA, depthQuantB
] = struct.unpack('iff', raw_header)
depth_img_scaled = depthQuantA / (
depth_img_raw.astype(np.float32) -
depthQuantB)
# filter max values
depth_img_scaled[depth_img_raw == 0] = 0
# depth_img_scaled provides distance in meters as f32
# for storing it as png, we need to convert it to 16UC1 again (depth in mm)
depth_img_mm = (depth_img_scaled *
1000).astype(np.uint16)
depth_img = depth_img_mm
else:
raise Exception("Decoding of '" +
sync_msg[sync_topic].format +
"' is not implemented!")
else:
if depth_fmt == "16UC1":
# assume that all 16bit image representations can be decoded by opencv
rawimgdata = sync_msg[sync_topic].data
depth_img = cv2.imdecode(
np.frombuffer(rawimgdata, np.uint8),
cv2.IMREAD_UNCHANGED)
else:
raise Exception("Decoding of '" +
sync_msg[sync_topic].format +
"' is not implemented!")
# write image
cv2.imwrite(
os.path.join(self.path_depth,
"depth_" + str(ref_time) + ".png"),
depth_img)
elif sync_topic == self.topic_depth and msg._type == Image._type:
depth_img = self.cvbridge.imgmsg_to_cv2(
sync_msg[sync_topic])
cv2.imwrite(
os.path.join(self.path_depth,
"depth_" + str(ref_time) + ".png"),
depth_img)
np.savetxt(os.path.join(self.export_path, "time.csv"),
ref_times,
fmt="%i")
if len(camera_poses) > 0:
np.savetxt(os.path.join(self.export_path, "camera_pose.csv"),
camera_poses,
fmt="%.8f",
header="px py pz qw qx qy qz",
delimiter=" ",
comments="")
else:
print(
"No camera poses have been extracted. Make sure that the given reference frame '"
+ self.ref_frame + "' exists.")
if len(joint_states) > 0:
np.savetxt(os.path.join(self.export_path, "joints.csv"),
joint_states,
fmt="%.8f",
header=(" ").join(full_joint_list_sorted),
delimiter=" ",
comments="")
print("done")
def __del__(self):
# close log file
if hasattr(self, 'bag'):
self.bag.close()
class Worker(QThread):
send_camera_view = Signal(object)
def __init__(self, parent = None, tfnet = None):
super(Worker, self).__init__()
#self.main = parent
self.sub = None
self.tfnet = tfnet
self.bridge = CvBridge()
self.predic_flag = 0
self.resize_width = 890
self.resize_height = 470
self.cappub = rospy.Publisher('/boxinfo_topic', YOLOBoxInfo, queue_size = 1)
def __del__(self):
print("============================= End Camera Thread ================================\n\n")
def run(self):
self.sub = rospy.Subscriber("/camera_topic", CompressedImage, self.callback)
def stop(self):
if not self.sub == None:
self.sub.unregister()
def callback(self, data):
try:
self.cv_image = self.bridge.compressed_imgmsg_to_cv2(data)# encoding
#self.results = self.tfnet.return_predict(self.cv_image)
#self.cv_image = self.boxing(self.cv_image, self.results)
self.cv_image = cv2.resize(self.cv_image, dsize = (self.resize_width, self.resize_height))
self.stime = time.time()
if(self.predic_flag == 0):
self.results = self.tfnet.return_predict(self.cv_image)
self.cv_image = self.boxing(self.cv_image, self.results)
#print('FPS {:.1f}'.format(1 / (time.time() - self.stime)))
self.predic_flag = 0
else:
self.predic_flag = 1
self.height, self.width, self.channel = self.cv_image.shape
self.cv_image = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2RGB)
self.bytesPerLine = 3 * self.width
self.qt_image = QImage(self.cv_image, self.width, self.height, self.bytesPerLine, QImage.Format_RGB888)
self.send_camera_view.emit(self.qt_image)
except CvBridgeError as e:
print("CvBridge err", e)
def boxing(self, original_img, predictions):
self.newImage = np.copy(original_img)
#self.bound_cnt = 0
self.bound_flag = 0
for result in predictions:
self.top_x = result['topleft']['x']
self.top_y = result['topleft']['y']
self.btm_x = result['bottomright']['x']
self.btm_y = result['bottomright']['y']
self.confidence = result['confidence']
self.label = result['label']# + " " + str(round(self.confidence, 3))
self.class_label = result['label']
#self.text = '{}: {:.0f}%'.format(self.class_label, self.confidence * 100)
if self.class_label == 'bookshelf' and (self.btm_y - self.top_y) * (self.btm_x - self.top_x) >= self.resize_width * self.resize_height * 0.2:
self.bound_flag = True
if self.class_label == 'bookshelf':
self.newImage = cv2.rectangle(self.newImage, (self.top_x, self.top_y), (self.btm_x, self.btm_y), (255, 0, 0), 3)
self.newImage = cv2.putText(self.newImage, self.label, (self.top_x, self.top_y-5), cv2.FONT_HERSHEY_COMPLEX_SMALL , 0.8, (0, 230, 0), 1, cv2.LINE_AA)
elif self.class_label == 'desktop'and self.confidence >=0.8:
self.newImage = cv2.rectangle(self.newImage, (self.top_x, self.top_y), (self.btm_x, self.btm_y), (0, 0, 255), 3)
self.newImage = cv2.putText(self.newImage, self.label, (self.top_x, self.top_y-5), cv2.FONT_HERSHEY_COMPLEX_SMALL , 0.8, (0, 230, 0), 1, cv2.LINE_AA)
else :
self.newImage = self.newImage
#self.newImage = cv2.putText(self.newImage, self.label, (self.top_x, self.top_y-5), cv2.FONT_HERSHEY_COMPLEX_SMALL , 0.8, (0, 230, 0), 1, cv2.LINE_AA)
if self.bound_flag:
self.sendData = YOLOBoxInfo()
self.sendData.tl_x = round(self.top_x / self.resize_width, 5)
self.sendData.tl_y = round(self.top_y / self.resize_height, 5)
self.sendData.br_x = round(self.btm_x / self.resize_width, 5)
self.sendData.br_y = round(self.btm_y / self.resize_height, 5)
self.sendData.confidence = round(self.confidence.item(), 5)
self.cappub.publish(self.sendData)
#print("box_info_publishing~~!!")
return self.newImage
'''
class OpenPosePreviewNode(Node):
openpose_wrapper: OpenPoseWrapper
def __init__(self):
super().__init__('openpose_node')
# ros params
is_debug_mode_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_BOOL,
description='If true, run debug mode.')
self.declare_parameter('is_debug_mode', False,
is_debug_mode_descriptor)
self.is_debug_mode: bool = self.get_parameter(
"is_debug_mode").get_parameter_value().bool_value
openpose_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_STRING,
description='The path of openpose project root.')
self.declare_parameter('openpose_root', '/openpose',
openpose_descriptor)
openpose_root: str = self.get_parameter(
"openpose_root").get_parameter_value().string_value
is_image_compressed_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_BOOL,
description='Is input image compressed?')
self.declare_parameter('is_image_compressed', False,
is_image_compressed_descriptor)
is_image_compressed: bool = self.get_parameter(
"is_image_compressed").get_parameter_value().bool_value
image_node_descriptor = ParameterDescriptor(
type=ParameterType.PARAMETER_STRING,
description='The node name of input image.')
self.declare_parameter('image_node', '/image', image_node_descriptor)
image_node: str = self.get_parameter(
"image_node").get_parameter_value().string_value
self.openpose_wrapper = OpenPoseWrapper(openpose_root)
self.bridge = CvBridge()
# show info
self.get_logger().info('IsDebugMode : ' + str(self.is_debug_mode))
self.get_logger().info('OpenposeRoot : ' + openpose_root)
self.get_logger().info('ImageNode : ' + image_node)
self.get_logger().info('IsImageCompressed : ' +
str(is_image_compressed))
if self.is_debug_mode:
self._publisher = self.create_publisher(Image, '/openpose/preview',
10)
self._publisher_compressed = self.create_publisher(
CompressedImage, '/openpose/preview/compressed', 10)
self._pose_publisher = self.create_publisher(
PoseKeyPointsList, '/openpose/pose_key_points', 10)
if is_image_compressed:
self.subscription = self.create_subscription(
CompressedImage, image_node, self.get_img_compressed_callback,
10)
else:
self.subscription = self.create_subscription(
Image, image_node, self.get_img_callback, 10)
def publish_from_img(self,
img: np.ndarray,
timestamp: Time,
frame_id: str = ""):
result = self.openpose_wrapper.body_from_image(img)
if self.is_debug_mode:
result_image: Image = self.bridge.cv2_to_imgmsg(
result.cvOutputData, "rgb8")
result_image_compressed: CompressedImage = self.bridge.cv2_to_compressed_imgmsg(
result.cvOutputData)
result_image.header.stamp = timestamp
result_image.header.frame_id = frame_id
result_image_compressed.header.stamp = timestamp
result_image_compressed.header.frame_id = frame_id
self._publisher.publish(result_image)
self._publisher_compressed.publish(result_image_compressed)
# Convert to KeyPointsList
pose_key_points_list_obj = PoseKeyPointsList()
if isinstance(result.poseKeypoints, np.ndarray):
pose_key_points_list = []
for result_pose_key_points in result.poseKeypoints:
pose_key_points = []
for result_pose_key_point in result_pose_key_points:
x, y, score = result_pose_key_point
pose_key_points.append(
PoseKeyPoint(x=x.item(),
y=y.item(),
score=score.item()))
pose_key_points_list.append(
PoseKeyPoints(pose_key_points=pose_key_points))
pose_key_points_list_obj = PoseKeyPointsList(
pose_key_points_list=pose_key_points_list)
pose_key_points_list_obj.header.stamp = timestamp
pose_key_points_list_obj.header.frame_id = frame_id
self._pose_publisher.publish(pose_key_points_list_obj)
def get_img_callback(self, image_raw: Image) -> None:
try:
print('[' + str(datetime.datetime.now()) + '] Image received',
end='\r')
image: np.ndarray = self.bridge.imgmsg_to_cv2(image_raw)
self.publish_from_img(image, image_raw.header.stamp,
image_raw.header.frame_id)
except Exception as err:
self.get_logger().error(err)
def get_img_compressed_callback(self, image_raw: CompressedImage) -> None:
try:
print('[' + str(datetime.datetime.now()) +
'] Compressed image received',
end='\r')
image: np.ndarray = self.bridge.compressed_imgmsg_to_cv2(image_raw)
self.publish_from_img(image, image_raw.header.stamp,
image_raw.header.frame_id)
except Exception as err:
self.get_logger().error(err)
class ImageExporter():
def __init__(self, FlAGS):
# Setup extraction directories
self.bag_dir = FLAGS.bag_dir
if FLAGS.save_dir:
self.save_dir = FLAGS.save_dir
else:
self.save_dir = FLAGS.bag_dir.rstrip('/') + "_FRAMES"
print("\nSaving to:", self.save_dir)
self.frames_dir = os.path.join(self.save_dir, "frames")
if not os.path.exists(self.save_dir):
os.makedirs(self.save_dir)
if not os.path.exists(self.frames_dir):
os.makedirs(self.frames_dir)
# To convert ROS images to OpenCV images so they can be saved.
self.bridge = CvBridge()
self.skip_rate = FLAGS.skip_rate
# Inialize csv logger
self.csv_logger = CSVLogger(self.save_dir, "frames.csv", FIELDS)
def process_frames(self):
print("\nMining Frames....\o/ \o/ \o/ \o/....... \n")
rosbag_files = sorted(glob.glob(os.path.join(self.bag_dir, "*.bag")))
# Init extraction loop
frame_count = 0
msg_count = 0
last_key_press = None
key_press_msgs = []
frame_type_count = {key: 0 for key in KEY_PRESS_LIST}
# Iterate through bags
rosbag_files = sorted(glob.glob(os.path.join(self.bag_dir, "*.bag")))
for bag_file in tqdm(rosbag_files, unit='bag'):
# Open bag file. If corrupted skip it
try:
with rosbag.Bag(bag_file, 'r') as bag:
# Check if desired topics exists in bags
recorded_topics = bag.get_type_and_topic_info()[1]
if not all(topic in recorded_topics
for topic in (CAR_STATE_TOPIC,
IMAGE_STREAM_TOPIC)):
print("ERROR: Specified topics not in bag file:",
bag_file, ".Skipping bag!")
continue
# Get key presses timings
for topic, msg, t in bag.read_messages():
if topic == CAR_STATE_TOPIC:
if msg.keypressed in KEY_PRESS_LIST:
if last_key_press is None:
last_key_press = msg
elif msg.header.stamp.to_sec(
) - last_key_press.header.stamp.to_sec() > 0.5:
key_press_msgs.append(msg)
last_key_press = msg
# Iterate through Image msgs if there are keypresses of interest
if key_press_msgs:
print("Extracting Frames from:", bag_file)
gps = ""
v_ego = 0.0
# Extract frames based on key press timings
for topic, msg, t in bag.read_messages():
# Add frames to buffer for selection
if topic == IMAGE_STREAM_TOPIC:
for key_press_msg in key_press_msgs:
if abs(msg.header.stamp.to_sec() - key_press_msg.header.stamp.to_sec()) <= 1 \
and msg_count % self.skip_rate == 0:
cv_image = self.bridge.compressed_imgmsg_to_cv2(
msg, "bgr8")
image_name = "frame%04d.jpg" % frame_count
cv2.imwrite(
os.path.join(
self.frames_dir, image_name),
cv_image)
frame_count += 1
# Update counts
frame_type_count[
key_press_msg.keypressed] += 1
# Write to csv
log_dict = dict()
log_dict["bag"] = bag_file.split(
"/")[-1]
log_dict[
"time_sec"] = msg.header.stamp.secs
log_dict[
"time_nsec"] = msg.header.stamp.nsecs
log_dict["GPS"] = gps
log_dict["v_ego"] = v_ego
log_dict["key_event"] = chr(
key_press_msg.keypressed)
log_dict["frame"] = image_name
self.csv_logger.record(log_dict)
# Next frame
break
msg_count += 1
if topic == CAR_STATE_TOPIC:
gps = msg.GPS
v_ego = msg.v_ego
msg_count = 0
last_key_press = None
key_presses = []
except ROSBagException:
print("\n", bag_file, "Failed! || ")
print(str(ROSBagException.value), '\n')
continue
# Print Summary
print("\nFrames Extracted:", frame_count)
print("================================")
[
print("Frames from '%s' press:" % chr(key), frame_type_count[key])
for key in KEY_PRESS_LIST
]
class MyNode(DTROS):
def __init__(self, node_name):
# initialize the DTROS parent class
super(MyNode, self).__init__(node_name=node_name)
# construct publisher and subsriber
self.pub = rospy.Publisher('/duckiesam/chatter', String, queue_size=10)
self.sub_image = rospy.Subscriber(
"/duckiesam/camera_node/image/compressed",
CompressedImage,
self.find_marker,
buff_size=921600,
queue_size=1)
self.pub_image = rospy.Publisher('/duckiesam/modified_image',
Image,
queue_size=1)
self.sub_info = rospy.Subscriber("/duckiesam/camera_node/camera_info",
CameraInfo,
self.get_camera_info,
queue_size=1)
self.pub_move = rospy.Publisher("/duckiesam/joy_mapper_node/car_cmd",
Twist2DStamped,
queue_size=1)
self.leader_detected = rospy.Publisher("/duckiesam/detection",
BoolStamped,
queue_size=1)
#values for detecting marker
self.starting = 0
self.ending = 0
self.camerainfo = PinholeCameraModel()
self.bridge = CvBridge()
self.gotimage = False
self.imagelast = None
self.processedImg = None
self.detected = False
#values for calculating pose of robot
self.solP = False
self.rotationvector = None
self.translationvector = None
self.axis = np.float32([[0.0125, 0, 0], [0, 0.0125, 0],
[0, 0, -0.0375]]).reshape(-1, 3)
self.distance = None
self.angle_f = None
self.angle_l = None
#values for driving the robot
self.maxdistance = 0.2
self.speedN = 0
self.e_vB = 0
self.rotationN = 0
self.mindistance = 0.1
self.d_e = 0 #distance error
self.d_e_1 = 5
self.d_e_2 = 10
self.y2 = 0
self.controltime = rospy.Time.now()
self.Kp = 1
self.Ki = 0.1
self.Kd = 0
self.I = 0
self.Rp = 1
self.Ri = 1
self.Rd = 1
def initialvalues(self):
self.default_v = 0.22
self.maxdistance = 0.3
self.speedN = 0
self.e_vB = 0
self.rotationN = 0
self.mindistance = 0.2
self.d_e = 0 #distance error
self.d_e_1 = 5
self.d_e_2 = 10
self.y2 = 0
self.controltime = rospy.Time.now()
self.Kp = 1
self.Ki = 0.1
self.Kd = 0
self.I = 0
self.Rp = 1
self.Ri = 1
self.Rd = 1
#get camera info for pinhole camera model
def get_camera_info(self, camera_msg):
self.camerainfo.fromCameraInfo(camera_msg)
#step 1 : find the back circle grids using cv2.findCirclesGrid
##### set (x,y) for points and flag for detection
def find_marker(self, image_msg):
try:
self.imagelast = self.bridge.compressed_imgmsg_to_cv2(
image_msg, "bgr8")
except CvBridgeError as e:
print(e)
if self.gotimage == False:
self.gotimage = True
#time checking
self.starting = rospy.Time.now()
#from_last_image = (self.starting - self.ending).to_sec()
gray = cv2.cvtColor(self.imagelast, cv2.COLOR_BGR2GRAY)
detection, corners = cv2.findCirclesGrid(gray, (7, 3))
self.processedImg = self.imagelast.copy()
cmd = Twist2DStamped()
cmd.header.stamp = self.starting
if detection:
cv2.drawChessboardCorners(self.processedImg, (7, 3), corners,
detection)
self.detected = True
#self.controltime = rospy.Time.now()
twoone = []
for i in range(0, 21):
point = [corners[i][0][0], corners[i][0][1]]
twoone.append(point)
twoone = np.array(twoone)
self.originalmatrix()
self.gradient(twoone)
self.detected = self.solP
img_out = self.bridge.cv2_to_imgmsg(self.processedImg, "bgr8")
self.pub_image.publish(img_out)
self.find_distance()
###self.move(self.y2, self.angle_l, self.distance)
self.ending = rospy.Time.now()
else:
self.detected = False
img_out = self.bridge.cv2_to_imgmsg(gray, "bgr8")
self.pub_image.publish(img_out)
self.ending = rospy.Time.now()
cmd.v = 0
cmd.omega = 0
####self.pub_move.publish(cmd)
#step 2 : makes matrix for 3d original shape
def originalmatrix(self):
#coners and points
self.originalmtx = np.zeros([21, 3])
for i in range(0, 7):
for j in range(0, 3):
self.originalmtx[i + j * 7, 0] = 0.0125 * i - 0.0125 * 3
self.originalmtx[i + j * 7, 1] = 0.0125 * j - 0.0125
#step 3 : use intrinsic matrix and solvePnP, return rvec and tvec, print axis
def gradient(self, imgpts):
#using solvePnP to find rotation vector and translation vector and also find 3D point to the image plane
self.solP, self.rotationvector, self.translationvector = cv2.solvePnP(
self.originalmtx, imgpts, self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
if self.solP:
pointsin3D, jacoB = cv2.projectPoints(
self.originalmtx, self.rotationvector, self.translationvector,
self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
pointaxis, _ = cv2.projectPoints(
self.axis, self.rotationvector, self.translationvector,
self.camerainfo.intrinsicMatrix(),
self.camerainfo.distortionCoeffs())
self.processedImg = cv2.line(self.processedImg,
tuple(imgpts[10].ravel()),
tuple(pointaxis[0].ravel()),
(255, 0, 0), 2)
self.processedImg = cv2.line(self.processedImg,
tuple(imgpts[10].ravel()),
tuple(pointaxis[1].ravel()),
(0, 255, 0), 2)
self.processedImg = cv2.line(self.processedImg,
tuple(imgpts[10].ravel()),
tuple(pointaxis[2].ravel()),
(0, 0, 255), 3)
#textdistance = "x = %s, y = %s, z = %s" % (self.distance, self.angle_f, self.angle_l, self.y2)
#rospy.loginfo("%s" % textdistance)
#step 4 : find distance between robot and following robot print out distance and time
def find_distance(self):
#use tvec to calculate distance
tvx = self.translationvector[0]
tvy = self.translationvector[1]
tvz = self.translationvector[2]
self.distance = math.sqrt(tvx * tvx + tvz * tvz)
self.angle_f = np.arctan2(tvx[0], tvz[0])
R, _ = cv2.Rodrigues(self.rotationvector)
R_inverse = np.transpose(R)
self.angle_l = np.arctan2(
-R_inverse[2, 0],
math.sqrt(R_inverse[2, 1]**2 + R_inverse[2, 2]**2))
T = np.array([-np.sin(self.angle_l), np.cos(self.angle_l)])
tvecW = -np.dot(R_inverse, self.translationvector)
x_y = np.array([tvz[0], tvx[0]])
self.y2 = -np.dot(T, x_y) - 0.01 * np.sin(self.angle_l)
textdistance = "Distance = %s, Angle of Follower = %s, Angle of Leader = %s, y = %s" % (
self.distance, self.angle_f, self.angle_l, self.y2)
rospy.loginfo("%s" % textdistance)
#self.pub.publish(textdistance)
#step 5 : use joy mapper to control the robot PID controller
def move(self, y_to, angle_to, d):
#y_to is needed y value to be parallel to leader's center line
#angle_to is angle needed to rotate
#d is distance between required position and now position
cmd = Twist2DStamped()
time = rospy.Time.now()
cmd.header.stamp = time
dt = (time - self.controltime).to_sec()
if dt > 3:
if d < self.maxdistance:
cmd.v = 0
cmd.omega = 0
else:
self.d_e = d - self.maxdistance
error_d = (self.d_e - self.d_e_1) / dt
errorB = (self.d_e_1 - self.d_e_2) / dt
e_v = error_d - errorB
P = self.Kp * (e_v)
self.I = self.I + self.Ki * (e_v + self.e_vB) / 2 * dt
D = self.Kd * (e_v + self.e_vB) / dt
self.speedN = P + self.I + D
self.rotationN = self.Rp * (y_to) + self.Ri * (
angle_to) + self.Rd * (np.sin(angle_to))
cmd.v = self.speedN
cmd.omega = self.rotationN
self.e_vB = e_v
self.d_e_2 = self.d_e_1
self.d_e_1 = self.d_e
if self.d_e < 0.05 or self.speedN < 0:
cmd.v = 0
cmd.omega = 0
textdistance = "Velocity = %s, Rotation = %s" % (cmd.v, cmd.omega)
rospy.loginfo("%s" % textdistance)
self.pub_move.publish(cmd)
self.controltime = time
def bag2vid(bag_path, topic, output_path, preview=False):
bag = rosbag.Bag(bag_path, 'r')
# Check if topic is in bag
info = bag.get_type_and_topic_info()
if topic not in info.topics:
raise RuntimeError("Opps! topic not in bag!")
# Check image message type
msg_type = info.topics[topic].msg_type
supported_msgs = ["sensor_msgs/CompressedImage", "sensor_msgs/Image"]
if msg_type not in supported_msgs:
err_msg = "bag2vid only supports %s!" % " or ".join(supported_msgs)
raise RuntimeError(err_msg)
# Get image shape
image_shape = None
br = CvBridge()
for topic, msg, t in bag.read_messages(topics=[topic]):
if msg_type == "sensor_msgs/CompressedImage":
image = br.compressed_imgmsg_to_cv2(msg)
image_shape = image.shape
else:
image = br.imgmsg_to_cv2(msg)
image_shape = image.shape
break
# Create video writer
height = image_shape[0]
width = image_shape[1]
shape = (width, height)
encoding = cv2.VideoWriter_fourcc(*"MJPG")
fps = info.topics[topic].frequency
video = cv2.VideoWriter(output_path, encoding, fps, shape)
# Write out the video
index = 0.0
index_end = float(info.topics[topic].message_count)
for topic, msg, t in bag.read_messages(topics=[topic]):
if msg_type == "sensor_msgs/CompressedImage":
image = br.compressed_imgmsg_to_cv2(msg)
video.write(image)
else:
image = br.imgmsg_to_cv2(msg, "bgr8")
video.write(image)
# Preview image
if preview:
cv2.imshow("Video: " + topic, image)
cv2.waitKey(1)
# Print progress
p = int((index / index_end) * 100.0)
print("Converting topic [%s] in bag [%s] to video - progress: %.0f%%"
% (topic, bag_path, p))
sys.stdout.write("\033[F") # Move cursor up one line
sys.stdout.flush()
index += 1.0
# Print final progress
print("Converting topic [%s] in bag [%s] to video - progress: %.0f%%"
% (topic, bag_path, p))
video.release()
class CameraFocus(Node):
def __init__(self):
super().__init__('camera_focus')
self.rec = False
self.zoom = 350
self.bruit = 0.4
self.pos = {
'left_eye': 0,
'right_eye': 0,
}
self.Init = {
'left_eye': True,
'right_eye': True,
}
self.final_pos = {
'left_eye': -1,
'right_eye': -1,
}
self.current_zoom = {
'left_eye': 350,
'right_eye': 350,
}
self.Start = True
self.Move = False
self.Points = []
self.k = 0
self.bridge = CvBridge()
self.img = {
'left_eye': CompressedImage(),
'right_eye': CompressedImage(),
}
self.camera_subscriber_left = self.create_subscription(
CompressedImage, 'left_image',
self.listener_callback_left,
10)
self.camera_subscriber_right = self.create_subscription(
CompressedImage, 'right_image',
self.listener_callback_right,
10)
self.set_camera_focus_zoom_client = self.create_client(SetCameraFocusZoom,
'set_camera_focus_zoom')
while not self.set_camera_focus_zoom_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service set_camera_focus_zoom_client not available, waiting again...')
self.req = SetCameraFocusZoom.Request()
self.set_focus_2_cameras_client = self.create_client(Set2CamerasFocus,
'set_2_cameras_focus')
while not self.set_focus_2_cameras_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service set_focus_2_cameras_client not available, waiting again...')
self.req_focus_2_cam = Set2CamerasFocus.Request()
self.get_zoom_focus_client = self.create_client(GetCameraFocusZoom,
'get_camera_focus_zoom')
while not self.get_zoom_focus_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service get_zoom_focus_client not available, waiting again...')
self.req_zoom_focus = GetCameraFocusZoom.Request()
self.keyboard_listener = keyboard.Listener(on_press=self.on_press)
self.keyboard_listener.start()
self.t = threading.Thread(target=self.asserv, args=('right_eye', 'left_image'), daemon=True)
self.t2 = threading.Thread(target=self.asserv, args=('left_eye', 'left_image'), daemon=True)
self.e_init = threading.Event()
self.e_end = threading.Event()
self.t.start()
self.t2.start()
# self._log = {'left': [], 'right': []}
def listener_callback_left(self, msg):
# print ("in listener callback")
# print(type(msg))
self.img['left_image'] = msg
# self.img['left_image'] = self.bridge.compressed_imgmsg_to_cv2(msg)
# print("listen left", time.time())
# self.k += 1
# cv.imshow("video",self.img)
# print(type(self.img))
# print(self.img)
def listener_callback_right(self, msg):
self.img['right_image'] = msg
# self.img['right_image'] = self.bridge.compressed_imgmsg_to_cv2(msg)
# print("listen right", time.time())
# print("listen right")
def send_request_set_focus_zoom(self, name, zoom, focus):
self.req.name = name
self.req.zoom = zoom
self.req.focus = focus
self.future = self.set_camera_focus_zoom_client.call_async(self.req)
def send_request_set_focus_2_cam(self, left_focus, right_focus):
self.req_focus_2_cam.left_focus = left_focus
self.req_focus_2_cam.right_focus = right_focus
self.future_focus_2_cam = self.set_focus_2_cameras_client.call_async(self.req_focus_2_cam)
def send_request_get_focus_zoom(self, name):
self.req_zoom_focus.name = name
self.future_zoom_focus = self.get_zoom_focus_client.call_async(self.req_zoom_focus)
def asserv(self, eye, im):
res_max = 0 # meilleur résultat de canny obtenu
p_max = 0 # posistion du focus associée à res_max
pos_min = 0 # position minimale accessible du focus
pos_max = 0 # position maximale accessible du focus
borne_inf = 0 # borne haute de tolerance du bruit
borne_sup = 0 # borne basse de tolerance du bruit
pas = 1 # pas d'avance
canny = [] # liste de stockage des résultats donnés par canny
poses = [] # liste de stockage des positions associées à canny[]
seuil = 0 # seuil d'interet, en dessous de cette valeur il ne peut pas y avoir de pic
k = 0 # variable de numérotation des images
j = 0 # variable de numérotation des diagrammes
sem = 0
zoom_prec = self.zoom
First = True # Flag à True s'il stagit de la première itération
Stop = 0 # 0 = pas stop, 1 = 1er passage en stop, 2 = plus de 1 passage en stop
time.sleep(1)
img = self.bridge.compressed_imgmsg_to_cv2(self.img[im])
img_prec_bw = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
while(1):
# print("asserv " + str(eye) + " " + str(j))
# j += 1
self.send_request_get_focus_zoom(eye)
img = self.bridge.compressed_imgmsg_to_cv2(self.img[im])
# if self.rec:
# print(k)
# cv.imwrite("src/reachy_focus/rec/"+str(eye)+str(k)+".png", img)
# k+=1
# if len(self.Points) == 2:
# roi = img[self.Points[0][1]:self.Points[1][1], self.Points[0][0]:self.Points[1][0]]
# imgBW = cv.cvtColor(roi, cv.COLOR_BGR2GRAY)
# else:
imgBW = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
if self.Start is True:
# # tp1 = time.time()
# # img_bw = cv.cvtColor(img[im], cv.COLOR_BGR2GRAY)
# t0 = time.time()
diff = cv.subtract(img_prec_bw, imgBW)
ret, diff = cv.threshold(diff, 20, 255, cv.THRESH_BINARY)
img_prec_bw = imgBW
sum = cv.integral(diff) # calcul la somme cumulée de touts les pixel dans l'image
move = sum[-1][-1]/(diff.shape[0]*diff.shape[1])
# t1 = time.time()
# print(t1 - t0)
# print(move)
if move >= 10 and sem < 3:
sem += 1
elif move < 10 and sem > -1:
sem -= 1
if sem >= 0 :
self.Move = True
else :
self.Move = False
# print(str(eye) +" " +str(k))
#cv.imwrite("src/reachy_focus/rec/"+str(eye)+str(self.pos[eye])+".png", img)
#k+=1
# print(str(eye)+": "+str(time.time()-tp1))
# print("move ="+ str(move))
# print ("sem = " + str(sem))
# print(str(eye) + " : "+ str(self.Move))
if self.Move is False:
res = Canny_Sharpness_function(imgBW)
canny.append(res)
poses.append(self.pos[eye])
# print (eye+"_res = "+str(res))
# print (eye+"_res max = "+str(res_max))
# print (eye+"_pos = " + str(self.pos[eye]))
self.test_response(self.future_zoom_focus)
try:
self.current_zoom[eye] = self.future_zoom_focus.result().zoom
except Exception as e:
pass
if self.current_zoom["left_eye"] == self.current_zoom["right_eye"]:
self.zoom = self.current_zoom["left_eye"]
print("zoom "+ str(self.zoom))
if self.zoom < 100:
self.bruit = 5
# if self.zoom != zoom_prec:
# self.Init['left_eye'] = True
# self.Init['right_eye'] = True
# zoom_prec = self.zoom
if self.Init[eye] is True:
# if seuil > res_max and res_max != 0:
# if res_max - 5 > 0:
# seuil = res_max - 5
# else:
# seuil = 0.5
# elif seuil < res_max-5:
# seuil = res_max-5
#print (seuil)
self.Init[eye] = False
First = True
Stop = 0
pos_min, pos_max = set_poses(self.zoom)
self.pos[eye] = pos_min
res_max = 0
pas = 1
if (eye == "left_eye" and self.Init["right_eye"] is False) or (eye == "right_eye" and self.Init["left_eye"] is False):
self.send_request_set_focus_2_cam(pos_min, pos_min) # déplacement des moteurs aux positions de départs
time.sleep(2)
print(str(eye) + " initialisation faite")
# self.test_response(self.future)
self.e_init.set()
self.e_init.clear()
else:
print(str(eye) + " attend l'initialisation")
self.e_init.wait()
elif Stop == 0:
# if len(self.Points) == 2:
# roi = img[im][self.Points[0][1]:self.Points[1][1], self.Points[0][0]:self.Points[1][0]]
# imgBW = cv.cvtColor(roi, cv.COLOR_BGR2GRAY )
# else :
# imgBW = cv.cvtColor(img[im], cv.COLOR_BGR2GRAY)
# cv.imwrite(str(pos[eye])+"_"+str(k)+".png",img[im])
# print("stop = 0")
if res > res_max:
# print("res > res_max")
res_max = res
p_max = self.pos[eye]
if First is True:
# print("First = True")
j += 1
canny = [] # liste de stockage des résultats donnés par canny
poses = []
tp1 = time.time()
First = False
borne_inf = res-self.bruit
borne_sup = res+self.bruit
# print ("b_min = "+ str(borne_inf)+"b_max = "+str(borne_sup))
self.pos[eye] = move_to(pos_min, pos_max, self.pos[eye], pas)
elif res < borne_inf or self.pos[eye] == pos_max:
# print ("res < borne_inf, p_max = " + str(p_max))
self.final_pos[eye] = p_max
tp2 = time.time()
print("!!!!!!!!!!!!time = " + str(tp2-tp1))
if (eye == "left_eye" and self.final_pos["right_eye"] > -1) or (eye == "right_eye" and self.final_pos["left_eye"] > -1):
Stop = 1
print(self.final_pos["right_eye"])
self.send_request_set_focus_2_cam(self.final_pos["left_eye"] - 30, self.final_pos["right_eye"] - 30)
time.sleep(0.5)
print(str(eye) + ": retour en arr")
self.send_request_set_focus_2_cam(self.final_pos["left_eye"], self.final_pos["right_eye"])
time.sleep(0.5)
print(str(eye) + ": pos max atteind, right_eye = " + str(self.final_pos["right_eye"])+" left_eye = " + str(self.final_pos["left_eye"]))
self.e_end.set()
# self.test_response(self.future)
self.pos[eye] = self.final_pos[eye]
self.final_pos[eye] = -1
self.e_end.clear()
else:
print(str(eye) + " attend")
self.e_end.wait()
self.pos[eye] = self.final_pos[eye]
self.final_pos[eye] = -1
Stop = 1
elif res <= seuil:
# print("res<seuil")
pas = 10
self.pos[eye] = move_to(pos_min, pos_max, self.pos[eye], pas)
elif res > borne_sup:
# print ("borne dépassée")
borne_inf = res-self.bruit
borne_sup = res+self.bruit
# print ("b_min = "+ str(borne_inf)+"b_max = "+str(borne_sup))
pas = 1
self.pos[eye] = move_to(pos_min, pos_max, self.pos[eye], pas)
else: # borne_inf<= res <=borne_sup:
# print ("dans les bornes")
if pas == 1:
pas = 5
# elif pas == 5:
# pas = 10
self.pos[eye] = move_to(pos_min, pos_max, self.pos[eye], pas)
self.send_request_set_focus_zoom(eye, self.zoom, self.pos[eye])
if self.pos[eye] == pos_min:
time.sleep(1)
if pas == 10:
time.sleep(0.15)
else:
time.sleep(0.1)
elif Stop == 1:
print ("stop = 1")
Stop = 2
# if res > res_max :
res_max = res
# elif ( res < 0.2*res_max or res > 2.5*res_max):
# #elif (res_max < 2 and (res < 0.6*res_max or res > 1.5*res_max)) or (res_max >= 2 and (res < 0.8*res_max or res > 2*res_max)):
# print("Stop = 2 et image flou ")
# print ("refocus : res = "+str(res)+", res_max = " + str(res_max))
# # Stop = 0
# # First = True
# self.Init['left_eye'] = True
# self.Init['right_eye'] = True
# # self.pos[eye] = pos_min
# # res_max = 0
# # pas = 1
# if self.pos[eye] == pos_min:
# time.sleep(1)
# if pas == 10:
# time.sleep(1)
# else :
# time.sleep(1)
# self.test_response(self.future)
else:
time.sleep(0.04)
# if eye == "right_eye":
# plt.figure()
# plt.plot(poses,canny)
# plt.title("profil du focus")
# plt.xlabel("pas du focus de la lentille")
# plt.ylabel("resultat de la fonction de contraste")
# plt.savefig("src/reachy_focus/datas/"+str(eye)+"/canny_"+str(j)+".png")
def test_response(self, future):
i = 0
while(rclpy.ok()):
# print(i)
# i+=1
if future.done():
try:
response = future.result()
except Exception as e:
self.get_logger().info(
'Service call failed %r' % (e,))
# else:
# self.get_logger().info(
# f'Result {response.zoom}')
break
time.sleep(0.001)
def on_press(self, key): # callback function pynput appelée à l'activation d'une touche
# if str(key) == "Key.up": # augmentation du zoom de 1 pas
# if self.zoom + 1 <= 600:
# self.zoom += 1
# self.Init['left_eye'] = True
# self.Init['right_eye'] = True
# if str(key) == "Key.down": # diminution du zoom de 1 pas
# if (self.zoom - 1) > 0:
# self.zoom -= 1
# self.Init['left_eye'] = True
# self.Init['right_eye'] = True
# if str(key) == "'z'": # modificarion du zoom
# user_input = input('zoom: ')
# err = 1
# while(err == 1):
# try:
# self.zoom = int(user_input)
# if 0 <= self.zoom <= 600:
# err = 0
# self.Init['left_eye'] = True
# self.Init['right_eye'] = True
# else:
# user_input = input("le zoom doit être comprs entre 0 et 600, recommencez: ")
# except Exception as e:
# user_input = input("le zoom saisie n'est pas un int, recommencez: ")
# if str(key) == "'c'": # clear all, ne garde pas en mémoire la dernière ROI selectionnée
# print("clear all")
# self.Init['left_eye'] = True
# self.Init['right_eye'] = True
# if self.Start is False:
# self.Start = True
# self.Points = []
if str(key) == "'r'": # restart, relance la recherche en concervant la ROI selectionnée
print("restart the sequence")
self.Init['left_eye'] = True
self.Init['right_eye'] = True
if self.Start is False:
self.Start = True
# if str(key) == "'i'": # choix d'une zone d'interet
# print ("selectonnez une zonne d'intérêt ")
# Start = False
# Init[eye] = True
if str(key) == "'s'": # start/stop
if self.Start is True:
self.Start = False
print("Stop")
else:
self.Start = True
print("Start")
class ImageOverlayControl:
def __init__(self):
self.scale_text = 0
self.scale_percentage = 0.4
self.heading = 0
self.depth = 0
self.roll = 0
self.pool = ThreadPool(3)
self.img = np.ones((800, 1100, 3), dtype=np.uint8) * 128
current_dir = os.path.dirname(os.path.abspath(__file__))
self.roll_img = load_image('%s/images/KREN_Arrow.png' % current_dir)
self.roll_img_bg = resize_image(
load_image('%s/images/KREN_Grad.png' % current_dir),
self.scale_percentage)
self.bridge = CvBridge()
rospy.init_node('image_overlayer')
self.roll = 0
self.depth = 0
self.target_depth = 0
self.heading = 0
self.target_heading = 0
self.roll_sub = rospy.Subscriber("rpy/roll", Float32,
self.roll_callback)
self.heading_sub = rospy.Subscriber("rpy/yaw", Float32,
self.heading_callback)
self.depth_sub = rospy.Subscriber("depth", Float32,
self.depth_callback)
self.target_depth_sub = rospy.Subscriber("target_depth", Float32,
self.target_depth_callback)
self.target_heading_sub = rospy.Subscriber(
"target_yaw", Float32, self.target_heading_callback)
self.frame_sub = rospy.Subscriber("/cam1/compressed", CompressedImage,
self.frame_callback)
self.frame_pub = rospy.Publisher("/overlay", Image, queue_size=10)
def roll_callback(self, data):
self.roll = data.data
def heading_callback(self, data):
self.heading = data.data
def target_heading_callback(self, data):
self.target_heading = data.data
if self.target_heading < 0:
self.target_heading += 360
rospy.logwarn(self.target_heading)
def target_depth_callback(self, data):
self.target_depth = data.data
def depth_callback(self, data):
self.depth = data.data
def frame_callback(self, data):
self.img = self.bridge.compressed_imgmsg_to_cv2(data)
#self.img = self.bridge.imgmsg_to_cv2(data)
result = self.overlay_telemetry()
result2 = self.bridge.cv2_to_imgmsg(result, "bgr8")
self.frame_pub.publish(result2)
def overlay_telemetry(self):
im_out = np.uint8(self.img.copy())
#im_out = self.img
def overlay_roll():
roll_img = rotate_image(self.roll_img, self.roll,
self.scale_percentage)
x = im_out.shape[1] / 2 - roll_img.shape[1] / 2
y = im_out.shape[0] / 2 - roll_img.shape[0] / 2
return overlay_image(roll_img, im_out, x, y)
def overlay_roll_bg():
x = im_out.shape[1] / 2 - self.roll_img_bg.shape[1] / 2
y = im_out.shape[0] / 2 - self.roll_img_bg.shape[0] / 2
return overlay_image(self.roll_img_bg, im_out, x, y)
def set_text():
font = 2
scale = 1
thick = 1
line_type = 4
color = tuple(np.ones(im_out.shape[2]) * 255)
loc = (int(20), int(30))
cv2.putText(im_out,
"Depth: %.2f (%.2f)" % (self.depth, self.target_depth),
loc, font, scale, color, int(thick), line_type)
loc = (im_out.shape[1] / 2 - 5, im_out.shape[0] - 15)
cv2.putText(im_out,
"%i (%i)" % (self.heading % 360, self.target_heading),
loc, font, scale, color, int(thick), line_type)
roll_r = self.pool.apply_async(overlay_roll, ())
roll_bg_r = self.pool.apply_async(overlay_roll_bg, ())
text_r = self.pool.apply_async(set_text, ())
roll_r.get()
roll_bg_r.get()
text_r.get()
return im_out
class Tracker:
def __init__(self, name):
self.topic_root = name
self.cam_model = miro.utils.CameraModel()
#Using the default size of camera
self.frame_w = 0
self.frame_h = 0
self.pose = Pose2D()
self.velocity = TwistStamped()
# Arrays to hold image topics
self.cam_left_image = None
self.cam_right_image = None
# Create object to convert ROS images to OpenCV format
self.image_converter = CvBridge()
self.direction_keeper = DirectionKeeper(self.topic_root)
self.data_resolver = DataResolver()
self.global_planner = node_path_planning.PathPlanner(name)
self.is_activated = True
self.safety_controller = SafetyController(name)
#publisher
self.velocity_pub = rospy.Publisher(self.topic_root +
"control/cmd_vel",
TwistStamped,
queue_size=0)
self.push_pub = rospy.Publisher(self.topic_root + "core/push",
miro.msg.push,
queue_size=0)
#suscriber
self.cam_left_sub = rospy.Subscriber(self.topic_root +
"sensors/caml/compressed",
CompressedImage,
self.cam_left_callback,
queue_size=1,
buff_size=52428800)
self.cam_right_sub = rospy.Subscriber(self.topic_root +
"sensors/camr/compressed",
CompressedImage,
self.cam_right_callback,
queue_size=1,
buff_size=52428800)
self.pos_sub = rospy.Subscriber(self.topic_root + "sensors/body_pose",
Pose2D,
self.pose_callback,
queue_size=1,
buff_size=52428800)
self.sensors_sub = rospy.Subscriber(
self.topic_root + "sensors/package", miro.msg.sensors_package,
self.callback_sensors)
self.current_mes = np.array((2, 1), np.float32)
self.current_pre = np.array((2, 1), np.float32)
self.kalman = cv2.KalmanFilter(4, 2)
self.kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]],
np.float32)
self.kalman.transitionMatrix = np.array(
[[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32)
self.kalman.processNoiseCov = np.array(
[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32) * 0.003
self.kalman.measurementNoiseCov = np.array([[1, 0], [0, 1]],
np.float32) * 1
#call back function for left camera
def cam_left_callback(self, ros_image):
try:
self.cam_left_image = self.image_converter.compressed_imgmsg_to_cv2(
ros_image, "bgr8")
im_h, im_w = self.cam_left_image.shape[:2]
if self.frame_w != im_w and self.frame_h != im_h:
self.frame_w, self.frame_h = im_w, im_h
self.cam_model.set_frame_size(self.frame_w, self.frame_h)
except CvBridgeError as e:
print("Conversion of left image failed \n")
print(e)
#call back function for right camera
def cam_right_callback(self, ros_image):
try:
self.cam_right_image = self.image_converter.compressed_imgmsg_to_cv2(
ros_image, "bgr8")
im_h, im_w = self.cam_right_image.shape[:2]
if self.frame_w != im_w and self.frame_h != im_h:
self.frame_w, self.frame_h = im_w, im_h
self.cam_model.set_frame_size(self.frame_w, self.frame_h)
except CvBridgeError as e:
print("Conversion of right image failed \n")
print(e)
def pose_callback(self, msg):
self.pose = msg
def callback_sensors(self, msg):
self.sensors_pack = msg
#monocular way for tracking
def tracking_motion_s(self, bbox, image, cam_index):
#check the opencv version
(major_ver, minor_ver, subminor_ver) = (cv.__version__).split('.')
# Set up tracker.
# Instead of MIL, you can also use
tracker_types = [
'BOOSTING', 'MIL', 'KCF', 'TLD', 'MEDIANFLOW', 'GOTURN', 'MOSSE',
'CSRT'
]
#choose the tracker using the index in the list above
tracker_type = tracker_types[3]
if tracker_type == 'BOOSTING':
tracker = cv.TrackerBoosting_create()
if tracker_type == 'MIL':
tracker = cv.TrackerMIL_create()
if tracker_type == 'KCF':
tracker = cv.TrackerKCF_create()
if tracker_type == 'TLD':
tracker = cv.TrackerTLD_create()
if tracker_type == 'MEDIANFLOW':
tracker = cv.TrackerMedianFlow_create()
if tracker_type == 'GOTURN':
tracker = cv.TrackerGOTURN_create()
if tracker_type == 'MOSSE':
tracker = cv.TrackerMOSSE_create()
if tracker_type == 'CSRT':
tracker = cv.TrackerCSRT_create()
bbox = list(bbox)
bbox[0] = bbox[0] if bbox[0] > 0 else 0
bbox[1] = bbox[1] if bbox[1] > 0 else 0
bbox[2] = bbox[2] if bbox[2] < 640 - bbox[0] else (640 - bbox[0] - 1)
bbox[3] = (360 - bbox[1] - 1) if bbox[3] > (360 - bbox[1]) else bbox[3]
bbox = tuple(bbox)
print("bbox:------------------------------->", bbox)
# bbox[0] = 0 if bbox[0]< 0 else bbox[0]
# bbox[1] = 0 if bbox[1]< 0 else bbox[1]
# bbox[2] = (640 - bbox[0] -1) if bbox[2] > (640 -bbox[0]) else bbox[2]
# bbox[3] = (360 - bbox[1] -1) if bbox[3] > (360 -bbox[1]) else bbox[3]
# bbox = tuple(bbox)
#get the first bbox in the first frame from the detection module
ok = tracker.init(image, bbox)
#use a counter to calculate the overall frames have been updated
frame_count = 0
center_pos = None
object_x = None
angle = 0
old_t_pos = None
t_move_distance = 0
old_time = 0
t_pos = None
now = 0
beta = None
r_pos = [0.0, 0.0]
old_r_pos = None
#get the pose from the topic, the pose topic only exists in the simulator
pose = np.array([self.pose.x, self.pose.y, self.pose.theta])
angular_vel = []
angle_lst = []
while True:
self.is_activated = self.safety_controller.sonar_control(0.15)
if self.is_activated == False:
print "Too close!!!"
break
# print("self.pose.x-------->:", self.pose)
if cam_index == 0:
#get the output of the image from camera
time.sleep(0.1)
frame = self.cam_left_image.copy()
elif cam_index == 1:
#get the output of the image from camera
time.sleep(0.1)
frame = self.cam_right_image.copy()
if frame is None:
print "no image is readed"
break
# Start now, calculate the time.
now = cv.getTickCount()
ok, newbox = tracker.update(frame)
# Calculate Frames per second (FPS)
fps = cv.getTickFrequency() / (cv.getTickCount() - now)
box = list(newbox)
#get the center pos of the newbox
center_pos = [box[0] + box[2] / 2, box[1] + box[3] / 2]
object_x = int(center_pos[0])
if ok:
p1 = (int(newbox[0]), int(newbox[1]))
p2 = (int(newbox[0] + newbox[2]), int(newbox[1] + newbox[3]))
# cv.rectangle(frame, p1, p2, (200,0,0))
cv.rectangle(frame, p1, p2, (255, 0, 0), 2, 1)
angle, beta, t_pos, direction, center_line = self.direction_keeper.diret_keeper_s(
center_pos, cam_index, pose)
print("actual angle---------->:", angle)
frame = cv.line(frame, (object_x, 180), (object_x, 360),
(255, 0, 0), 1)
frame = cv.line(frame, (object_x, 180), (center_line[0], 180),
(0, 0, 255), 2)
# frame = cv.line(frame,(object_x,180),(320,180),(0,0,255),2)
if angle != 0:
self.velocity = self.direction_keeper.reguralize_angle(
angle, direction, False)
#after the angle has been corrected, the t_pos above will be None, so we should
#tansform the target to the world coordination system. And this can be a point for
#later heading toward and tracking the moving object. I set a accepted error range
#at 0.8 degree for saving time, because the angle will swing between the zero degree,
# #so, we might not always get the lowest value, like the gradient in machine learning.
#considering the time limitation, we cannot allow the robot to regularize
# the angle at a extremelly accurate stage,so here i just give him a large
#angle to push the velocity.
# old_second = datetime.datetime.now().second
if old_t_pos is not None:
# pose = np.array([self.pose.x,self.pose.y,self.pose.theta])
print("pose--------->", pose)
vel_r, cos_l, sin_l = self.global_planner.path_planning(
cam_index, center_pos, old_t_pos, beta, pose, now,
old_time, self.velocity.twist.angular.z)
print("pose-----------------after>", pose)
spd = np.array(self.sensors_pack.wheel_speed_opto.data)
print "spd--->", spd
twist = self.sensors_pack.odom.twist.twist
dr = twist.linear.x
dtheta = twist.angular.z
#Here is the measurement of the state space(position, velocity)
# mes_array = np.array([pose.x,pose.y, dr *cos_l,dr *sin_l])
# mes_array = np.array([pose[0],pose[1]])
mes_array = np.array([old_t_pos[0], old_t_pos[1]])
self.current_mes = np.array([[np.float32(mes_array[0])],
[np.float32(mes_array[1])]])
estimated = self.kalman.correct(self.current_mes)
print "corrected estimation from KF", estimated
self.current_pre = self.kalman.predict()
print "Prediciton from Kalman Filter", self.current_pre
# print ""
#Here is the prediction of the state space, compare the prediction and the Kalman prediction
r_pos[0] = r_pos[0] + vel_r * cos_l * 2.0
r_pos[1] = r_pos[1] + vel_r * sin_l * 2.0
pre_array = np.array(
[r_pos[0], r_pos[1], vel_r * cos_l, vel_r * sin_l])
print "Prediction from calculation", pre_array
print("pose-----------------after>", pose)
# current_second = datetime.datetime.now().second
# if abs(current_second - old_second):
# continue
else:
# Tracking failure
cv.putText(frame, "Tracking failure detected", (100, 80),
cv.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)
cv2.destroyAllWindows()
break
# Display tracker type on frame
cv.putText(frame, tracker_type + " Tracker", (100, 20),
cv.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
# Display FPS on frame
cv.putText(frame, "FPS : " + str(int(fps)), (100, 50),
cv.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
im_h = np.size(frame, 0)
im_w = np.size(frame, 1)
im_centre_h = im_h / 2.0
im_centre_w = im_w / 2.0
#draw the calibration lines at x and y axis in the middle of the image.
cv.line(frame, (0, int(round(im_centre_h))),
(im_w, int(round(im_centre_h))), (100, 100, 100), 1)
cv.line(frame, (int(round(im_centre_w)), 0),
(int(round(im_centre_w)), im_h), (100, 100, 100), 1)
# old_r_pos = r_pos
old_t_pos = t_pos
old_time = now
#set the frames number for this tracking module to update, it means a epoch
frame_count += 1
if frame_count == 200:
cv.destroyAllWindows()
break
if cam_index == 0:
cv.imshow("left_tracking", frame)
elif cam_index == 1:
cv.imshow("right_tracking", frame)
k = cv.waitKey(1) & 0xff
if k == 27: break # esc pressed
angular_vel.append(self.velocity.twist.angular.z)
angle_lst.append(angle)
return angular_vel, angle_lst
def tracking_motion(self, l_bbox, r_bbox, l_image, r_image):
#check the opencv version
# (major_ver, minor_ver, subminor_ver) = (cv.__version__).split('.')
l_tracker = cv.TrackerBoosting_create()
r_tracker = cv.TrackerBoosting_create()
# Set up tracker.
# Instead of MIL, you can also use
tracker_types = [
'BOOSTING', 'MIL', 'KCF', 'TLD', 'MEDIANFLOW', 'GOTURN', 'MOSSE',
'CSRT'
]
#choose the tracker using the index in the list above
l_tracker_type = tracker_types[3]
r_tracker_type = tracker_types[2]
if l_tracker_type == 'BOOSTING':
l_tracker = cv.TrackerBoosting_create()
elif r_tracker_type == 'BOOSTING':
r_tracker = cv.TrackerBoosting_create()
if l_tracker_type == 'MIL':
l_tracker = cv.TrackerMIL_create()
elif r_tracker_type == 'MIL':
r_tracker = cv.TrackerMIL_create()
if l_tracker_type == 'KCF':
l_tracker = cv.TrackerKCF_create()
elif r_tracker_type == 'KCF':
r_tracker = cv.TrackerKCF_create()
if l_tracker_type == 'TLD':
l_tracker = cv.TrackerTLD_create()
elif r_tracker_type == 'TLD':
r_tracker = cv.TrackerTLD_create()
if l_tracker_type == 'MEDIANFLOW':
l_tracker = cv.TrackerMedianFlow_create()
elif r_tracker_type == 'MEDIANFLOW':
r_tracker = cv.TrackerMedianFlow_create()
if l_tracker_type == 'GOTURN':
l_tracker = cv.TrackerGOTURN_create()
elif r_tracker_type == 'GOTURN':
r_tracker = cv.TrackerGOTURN_create()
if l_tracker_type == 'MOSSE':
l_tracker = cv.TrackerMOSSE_create()
elif r_tracker_type == 'MOSSE':
r_tracker = cv.TrackerMOSSE_create()
if l_tracker_type == 'CSRT':
l_tracker = cv.TrackerCSRT_create()
elif r_tracker_type == 'CSRT':
r_tracker = cv.TrackerCSRT_create()
l_bbox = list(l_bbox)
l_bbox[0] = l_bbox[0] if l_bbox[0] > 0 else 0
l_bbox[1] = l_bbox[1] if l_bbox[1] > 0 else 0
l_bbox[2] = l_bbox[2] if l_bbox[2] < 640 - l_bbox[0] else (640 -
l_bbox[0] -
1)
l_bbox[3] = (360 - l_bbox[1] -
1) if l_bbox[3] > (360 - l_bbox[1]) else l_bbox[3]
l_bbox = tuple(l_bbox)
print("l_bbox:------------------------------->", l_bbox)
r_bbox = list(r_bbox)
r_bbox[0] = r_bbox[0] if r_bbox[0] > 0 else 0
r_bbox[1] = r_bbox[1] if r_bbox[1] > 0 else 0
r_bbox[2] = r_bbox[2] if r_bbox[2] < 640 - r_bbox[0] else (640 -
r_bbox[0] -
1)
r_bbox[3] = (360 - r_bbox[1] -
1) if r_bbox[3] > (360 - r_bbox[1]) else r_bbox[3]
r_bbox = tuple(r_bbox)
print("r_bbox:------------------------------->", r_bbox)
#get the first bbox in the first frame from the detection module
time.sleep(0.01)
l_ok = l_tracker.init(l_image, l_bbox)
time.sleep(0.01)
r_ok = r_tracker.init(r_image, r_bbox)
#use a counter to calculate the overall frames have been updated
frame_count = 0
l_center_pos = None
r_center_pos = None
object_x = None
angle = 0
# old = cv2.getTickCount()
angle_last = 0
# old_box = None
l_old_t_pos = None
r_old_t_pos = None
r_t_pos = None
l_t_pos = None
pose = np.array([self.pose.x, self.pose.y, self.pose.theta])
l_old_time = 0
r_old_time = 0
l_angular_vel = []
l_angle_lst = []
r_angular_vel = []
r_angle_lst = []
r_angle = 0
l_angle = 0
while True:
self.is_activated = self.safety_controller.sonar_control(0.15)
if self.is_activated == False:
print "Too close!!!"
break
#get the output of the image from camera
time.sleep(0.1)
l_frame = self.cam_left_image.copy()
#get the output of the image from camera
time.sleep(0.1)
r_frame = self.cam_right_image.copy()
if l_frame is None and r_frame is None:
print "no image is readed"
break
# Start now, because we will need the time of updating frames by different
#trckers, so we store the time here seperately
l_now = cv.getTickCount()
l_ok, l_newbox = l_tracker.update(l_frame)
l_fps = cv.getTickFrequency() / (cv.getTickCount() - l_now)
r_now = cv.getTickCount()
r_ok, r_newbox = r_tracker.update(r_image)
r_fps = cv.getTickFrequency() / (cv.getTickCount() - r_now)
print("dif_fps---------------->:", l_fps - r_fps)
l_box = list(l_newbox)
r_box = list(r_newbox)
#get the center pos of the newbox
l_center_pos = [l_box[0] + l_box[2] / 2, l_box[1] + l_box[3] / 2]
l_object_x = int(l_center_pos[0])
#get the center pos of the newbox
r_center_pos = [r_box[0] + r_box[2] / 2, r_box[1] + r_box[3] / 2]
r_object_x = int(r_center_pos[0])
if l_ok and r_ok == False:
p1 = (int(l_newbox[0]), int(l_newbox[1]))
p2 = (int(l_newbox[0] + l_newbox[2]),
int(l_newbox[1] + l_newbox[3]))
# cv.rectangle(frame, p1, p2, (200,0,0))
cv.rectangle(l_frame, p1, p2, (255, 0, 0), 2, 1)
l_angle, l_beta, l_t_pos, direction, l_center_line = self.direction_keeper.diret_keeper(
l_center_pos, r_center_pos, pose)
cv.line(l_frame, (l_object_x, 180), (l_object_x, 360),
(255, 0, 0), 1)
l_frame = cv.line(l_frame, (l_object_x, 180),
(int(l_center_line[0]), 180), (0, 0, 255), 2)
# cv.line(l_frame,(l_object_x,180),(320,180),(0,0,255),2)
print("left_actual angle---------->:", l_angle)
if l_angle != 0:
self.velocity = self.direction_keeper.reguralize_angle(
l_angle, direction, True)
l_angular_vel.append(self.velocity.twist.angular.z)
# The final status is the robot's velocity lies on the sight line between the
#robot and the target. I set a small value for the accepted error range of degrees
# at 1 degree, in this case, the direction of the robot is almost directed forward
# to the target. Also, if the old target position is None, it means there is no postion
# info at the initiate stage, so we need to skip this result.
#considering the time limitation, we cannot allow the robot to regularize
# the angle at a extremelly accurate stage,so here i just give him a large
#angle to push the velocity.
if l_old_t_pos is not None:
#update the pose from the subscriber
# pose = np.array([self.pose.x,self.pose.y,self.pose.theta])
self.global_planner.path_planning(
0, l_center_pos, l_old_t_pos, l_beta, pose, l_now,
l_old_time, self.velocity.twist.angular.z)
elif l_ok != True:
# Tracking failure
cv.putText(l_frame, "Tracking failure detected", (100, 80),
cv.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)
pass
# Display tracker type on l_frame
cv.putText(l_frame, l_tracker_type + " Tracker", (100, 20),
cv.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
# Display FPS on l_frame
cv.putText(l_frame, "FPS : " + str(int(l_fps)), (100, 50),
cv.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
im_h = np.size(l_frame, 0)
im_w = np.size(l_frame, 1)
im_centre_h = im_h / 2.0
im_centre_w = im_w / 2.0
#draw the calibration lines at x and y axis in the middle of the image.
cv.line(l_frame, (0, int(round(im_centre_h))),
(im_w, int(round(im_centre_h))), (100, 100, 100), 1)
cv.line(l_frame, (int(round(im_centre_w)), 0),
(int(round(im_centre_w)), im_h), (100, 100, 100), 1)
# and l_ok == False
if r_ok and l_ok == False:
p1 = (int(r_newbox[0]), int(r_newbox[1]))
p2 = (int(r_newbox[0] + r_newbox[2]),
int(r_newbox[1] + r_newbox[3]))
# cv.rectangle(frame, p1, p2, (200,0,0))
cv.rectangle(r_frame, p1, p2, (255, 0, 0), 2, 1)
r_angle, r_beta, r_t_pos, direction, r_center_line = self.direction_keeper.diret_keeper(
l_center_pos, r_center_pos, pose)
cv.line(r_frame, (r_object_x, 180), (r_object_x, 360),
(255, 0, 0), 1)
cv.line(r_frame, (r_object_x, 180),
(int(r_center_line[0]), 180), (0, 0, 255), 2)
print("left_actual angle---------->:", r_angle)
if r_angle != 0:
self.velocity = self.direction_keeper.reguralize_angle(
r_angle, direction, True)
r_angular_vel.append(self.velocity.twist.angular.z)
# The final status is the robot's velocity lies on the sight line between the
#robot and the target. I set a small value for the accepted error range of degrees
# at 1 degree, in this case, the direction of the robot is almost directed forward
# to the target. Also, if the old target position is None, it means there is no postion
# info at the initiate stage, so we need to skip this result.
#considering the time limitation, we cannot allow the robot to regularize
# the angle at a extremelly accurate stage,so here i just give him a large
#angle to push the velocity.
if r_old_t_pos is not None:
# #update the pose from the subscriber
# pose = np.array([self.pose.x,self.pose.y,self.pose.theta])
self.global_planner.path_planning(
1, r_center_pos, r_old_t_pos, r_beta, pose, r_now,
r_old_time, self.velocity.twist.angular.z)
elif r_ok != True:
# Tracking failure
cv.putText(r_frame, "Tracking failure detected", (100, 80),
cv.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2)
cv2.destroyAllWindows()
break
# Display tracker type on r_frame
cv.putText(r_frame, r_tracker_type + " Tracker", (100, 20),
cv.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
# Display FPS on r_frame
cv.putText(r_frame, "FPS : " + str(int(r_fps)), (100, 50),
cv.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2)
im_h = np.size(r_frame, 0)
im_w = np.size(r_frame, 1)
im_centre_h = im_h / 2.0
im_centre_w = im_w / 2.0
#draw the calibration lines at x and y axis in the middle of the image.
cv.line(r_frame, (0, int(round(im_centre_h))),
(im_w, int(round(im_centre_h))), (100, 100, 100), 1)
cv.line(r_frame, (int(round(im_centre_w)), 0),
(int(round(im_centre_w)), im_h), (100, 100, 100), 1)
#set the frames number for this tracking module to update, it means a epoch is 50 frames
frame_count += 1
if frame_count == 200:
cv.destroyAllWindows()
break
#update the position of the target.
r_old_t_pos = r_t_pos
l_old_t_pos = l_t_pos
# angle_last = angle
l_old_time = l_now
r_old_time = r_now
cv.imshow("left_tracking", l_frame)
cv.imshow("right_tracking", r_frame)
k = cv.waitKey(1) & 0xff
if k == 27: break # esc pressed
l_angle_lst.append(l_angle)
r_angle_lst.append(r_angle)
return l_angle_lst, r_angle_lst, r_angular_vel, l_angular_vel
class AutoDrive:
def __init__(self):
self.mode = 0
self.left_lane = False
self.right_lane = False
self.center = 0
self.lastError = 0
self.MAX_VEL = 0.2
self.cam_img = np.zeros(shape=(360, 640, 3), dtype=np.uint8)
self.cam_img2 = np.zeros(shape=(480, 640, 3), dtype=np.uint8)
self.bridge = CvBridge()
self.go = False
self.version = 1
self.msg = []
# three-street
self.right_step = 1
# Construction
self.phase = 0
self.status = 0
self.check =0
# Parking
# Stop
self.Chadan = 0
self.Chadan_go = False
self.pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.mode_msg = UInt8()
self.sub_img1 = rospy.Subscriber('/camera1/usb_cam1/image_raw/compressed', CompressedImage,
self.callback1, queue_size=1)
self.sub_img2= rospy.Subscriber('/camera2/usb_cam2/image_raw/compressed', CompressedImage,
self.callback2, queue_size=1)
self.sub_detect_sign = rospy.Subscriber('/detect/traffic_sign', UInt8, self.mode_selector, queue_size=1)
self.sub_obstacle = rospy.Subscriber('/scan',LaserScan,self.obstacle_callback,queue_size=1)
def PD_control(self, kp =0.012,kd = 0.004):
error = self.center - 327
print('error : ', error)
angular_z = kp * error + kd * (error - self.lastError)
self.lastError = error
speed = min(self.MAX_VEL * ((1 - abs(error) / 320) ** 2.2), 0.2:)
if abs(error) > 200:
angular = -max(angular_z, -2.0 if angular_z < 0 else -min(angular_z,2.0))
else:
angular = -max(angular_z, -1.3 if angular_z < 0 else -min(angular_z,1.3))
return speed, angular
def callback1(self,img_data):
self.cam_img = self.bridge.compressed_imgmsg_to_cv2(img_data, "bgr8")
self.left_lane, self.right_lane, self.center = Line(self.cam_img,self.version).detect_lines()
def callback2(self,img_data):
self.cam_img2 = self.bridge.compressed_imgmsg_to_cv2(img_data,"bgr8")
def mode_selector(self, mode_msg):
# 1 left 2 right 4 construction 5 stop 6 parking 7 tunnel
if mode_msg == 2:
self.right_sign = True
def obstacle_callback(self,obstacle_msg):
self.msg = obstacle_msg
def construction(self):
msg = self.obstacle.msg
#print msg
speed = 0
angular = 0
if self.phase == 0:
# print 'Phase 1' , self.status
if self.status == 0:
speed = 0.2
if msg.ranges[90] < 0.3 and msg.ranges[90] != 0:
self.status = 1
else:
angular = 0
elif self.status == 1:
speed = 0.1
angular = 0.37
if msg.ranges[270] < 0.4 and msg.ranges[270] != 0:
self.phase += 1
self.status = 0
self.move(speed,angular)
elif self.phase == 1:
speed = 0.1
angular = -0.4
if msg.ranges[0] < 0.5 and msg.ranges[0] != 0:
self.check += 1
if self.check != 0 and msg.ranges[70] < 0.5 and msg.ranges[70] != 0:
self.phase += 1
self.move(speed,angular)
elif self.phase == 2:
speed = 0.12
angular = 0.36
if msg.ranges[200] < 0.5 and msg.ranges[200] != 0:
self.phase += 1
self.move(speed,angular)
elif self.phase == 3:
self.mode = 3
# print 'Phase 4'# rospy.sleep(rospy.Duration(2))
print('break construction')
#self.mode += 1
print('speed, angluar : ', speed,angular)
def run(self):
rospy.on_shutdown(self.myhook)
img = self.cam_img
img2 = self.cam_img2
speed = 0
angular = 0
if self.mode == 0 :
self.go = Light(img).find()
if self.go == True :
self.mode += 1
# green light && Left, Rigth Sign
if self.mode == 1 and self.go == True:
speed, angular = self.PD_control(kp = 0.012,kd = 0.004)
if self.mode_msg == 1:
self.mode = 2
if self.mode_msg == 2:
if self.right_step == 1:
self.move(0,0)
rospy.sleep(rospy.Duration(1))
self.move(0.1,-0.37)
rospy.sleep(rospy.Duration(3))
self.mode = 2
#Construction
if self.mode == 2 :
if self.mode_msg == 4:
self.construction()
# parking
if self.mode == 3:
if self.mode_msg ==6 :
self.execute_parking_mode_with_lidar(msg)
else:
speed, angular = self.PD_control(kp = 0.008,kd = 0.004)
self.move(speed,angular)
# Stop bar
if self.mode == 5 :
self.Chadan = Stopbar(img2).find()
#print(self.Chadan)
if self.Chadan > 1:
self.move(0,0)
self.Chadan_go = True
elif self.Chadan == 0 and self.Chadan_go == True:
self.mode =6
else:
speed, angular = self.PD_control(kp = 0.008,kd = 0.004)
self.move(speed,angular)
if self.mode == 6:
speed, angular = self.PD_control(kp = 0.008,kd = 0.004)
self.move(speed,angular)
#a = self.stopbar.find
#print(a)
# Tunnel
'''
if self.mode == 6 and self.mode_msg == 7:
if 0.05 < msg.ranges[290] <0.3:
if self.tunnel_step == 3:
os.system('roslaunch foscar_turtlebot3_autorace tunnel.launch')
os.system('rosnode kill /detect_signs')
os.system('rosnode kill /line_trace')
self.tunnel_step = 4
speed, angular = self.PD_control(kp = 0.008,kd = 0.004)
self.move(speed,angular)
'''
#print("traffic", self.go)
print("mode,sign ", self.mode, self.mode_msg)
print("left : ", self.left_lane)
print("right: ", self.right_lane)
print("center : ", self.center)
#print("speed : ",speed)
# print("angular : ", angular)
self.move(speed, angular)
def myhook(self):
twist = Twist()
twist.linear.x = 0
twist.angular.z = 0
self.pub_cmd_vel.publish(twist)
def move(self, speed, angular):
twist = Twist()
twist.linear.x = speed
twist.angular.z = angular
self.pub_cmd_vel.publish(twist)
if __name__ == '__main__':
rospy.init_node('line_trace')
foscar = AutoDrive()
time.sleep(1)
rate = rospy.Rate(100)
while not rospy.is_shutdown():
foscar.run()
rate.sleep()
def predictImages(config_file, weight_path, bag_file, bag_file_topics):
n = 0
#Create a publisher
image_pub = rospy.Publisher("image_inferencer", Image)
bridge = CvBridge()
#Initialize a node
rospy.init_node('video', anonymous=True)
#Make inferences on the images in the image_dir
ROOT_DIR = os.path.dirname(os.path.abspath(__file__))
# ** get configuration
config_client = ConfigParser()
config_client.read(config_file)
# ** set gpu
os.environ['CUDA_VISIBLE_DEVICES'] = config_client.get('gpu', 'gpu')
# ** Weights
if weight_path is None:
weight_path = config_client.get('inference', 'weight_path')
# ** Bag File
if bag_file is None:
bag_file = config_client.get('video_inference', 'bag_file_path')
bag = rosbag.Bag(bag_file, "r")
# ** Topics
if bag_file_topics is None:
bag_file_topics = config_client.get('video_inference',
'bag_file_topics')
# ** Where to store the predictions
inference_dir = config_client.get('video_inference', 'inference_dir')
if not os.path.exists(inference_dir):
os.mkdir(inference_dir)
# ** Input Sizes
input_width = config_client.getint('model', 'input_width')
input_height = config_client.getint('model', 'input_height')
# ** Number of outputs
num_outputs = config_client.getint('model', 'num_outputs')
print("Loading Model")
model = load_model(weight_path,
custom_objects={
'_hard_swish': _hard_swish,
'_relu6': _relu6
})
trajectory = Trajectory(input_width, input_height, num_outputs)
robotCenter = trajectory.robotCenter
robotWidth = trajectory.robotWidth
robotCenter = trajectory.robotCenter
robotLeft = trajectory.robotLeft
robotRight = trajectory.robotRight
robotFront = trajectory.robotFront #Front of the robot
robotCloseUp = trajectory.robotCloseUp #The very front of the robot
maxTranslation = trajectory.maxTranslation
prevFrameRotation = None
jitter = 0
#Run through the images and predict the free space and trajectory
stepSize = input_width // num_outputs
for topic, msg, t in bag.read_messages(topics=[bag_file_topics]):
#Exit on shutdown
if rospy.is_shutdown():
print('Shutting down inferenceer')
exit()
#Copy the frame to show processing
frame = bridge.compressed_imgmsg_to_cv2(msg)
frame = cv2.resize(frame, (input_width, input_height))
processed_frame = frame.copy()
#Normalize the input image
X = np.empty((1, input_height, input_width, 3), dtype='float32')
X[0, :, :, :] = image_augmentation(frame)
#Predict the free space
print("Predicting Frame: " + str(n))
prediction = model.predict(X)[0]
highestPoint = robotCenter
x = 0
for i in range(len(prediction)):
y = int(round(prediction[i] * input_height))
if y < highestPoint[1]:
highestPoint = (x, y)
#Draw circles on the original image to show where the predicted free space occurs
processed_frame = cv2.circle(processed_frame, (x, y), 1,
(0, 255, 0), -1)
x += stepSize
#Draw lines representing the sides of the robot
cv2.line(processed_frame,
(robotCenter[0] - robotWidth, robotCenter[1]),
(robotCenter[0] - robotWidth, robotFront), (0, 0, 255), 2)
cv2.line(processed_frame,
(robotCenter[0] + robotWidth, robotCenter[1]),
(robotCenter[0] + robotWidth, robotFront), (0, 0, 255), 2)
#Draw a line representing the boundary of the front of the robot
cv2.line(processed_frame, (robotLeft, robotFront),
(robotRight, robotFront), (0, 0, 255), 2)
#Calculate the trajectory of the robot
(translation, rotation) = trajectory.calculateTrajectory(prediction)
#Convert the trajectory percentages to the target point
(translation_x,
translation_y) = trajectory.trajectoryToPoint(translation, rotation)
#Display the magnitude and direction of the vector the robot should drive along
cv2.putText(processed_frame, "Rotation: " + str(rotation), (10, 50),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
cv2.putText(processed_frame, "Translation: " + str(translation),
(10, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
#Draw an arrowed line indicating the predicted trajectory of the robot
if rotation == -1 and translation == 0:
cv2.arrowedLine(processed_frame, (robotCenter[0], robotFront),
(input_width - 1, robotFront), (0, 0, 255), 2)
elif rotation == 1 and translation == 0:
cv2.arrowedLine(processed_frame, (robotCenter[0], robotFront),
(0, robotFront), (0, 0, 255), 2)
else:
cv2.arrowedLine(processed_frame, robotCenter,
(translation_x, translation_y), (0, 0, 255), 2)
n += 1
try:
image_pub.publish(bridge.cv2_to_imgmsg(processed_frame, "bgr8"))
except CvBridgeError as e:
print(e)
if prevFrameRotation is not None:
jitter += rotation - prevFrameRotation
prevFrameRotation = rotation
print("Jitter: " + str(jitter))
return
class Joystick(Node):
"""A ROS node to drive a *very* simple differential drive robot."""
keymap = {
'w': (1, 1),
'a': (-1, 1),
's': (-1, -1),
'd': (1, -1),
'W': (2, 2),
'A': (-2, 2),
'S': (-2, -2),
'D': (2, -2),
' ': (0, 0),
}
def __init__(self, stdin):
"""Initialize the Joystick node."""
super(Joystick, self).__init__(name='Joystick')
self.stdin = stdin
self.settings = None
self.left_publisher = ros.Publisher(TOPIC['WHEEL_LEFT'],
Int32,
queue_size=1)
self.right_publisher = ros.Publisher(TOPIC['WHEEL_RIGHT'],
Int32,
queue_size=1)
self.camera_topic = TOPIC['CAMERA_FEED']
self.bridge = CvBridge()
def init_node(self):
"""Perform custom Node initialization."""
ros.Subscriber(self.camera_topic, CompressedImage, self.image_handler)
ros.Timer(ros.Duration(0.1), self.callback)
sys.stdin = os.fdopen(self.stdin)
self.settings = termios.tcgetattr(sys.stdin)
new_attrs = self.settings[:]
new_attrs[3] = new_attrs[3] & ~(termios.ECHO | termios.ICANON)
termios.tcsetattr(sys.stdin.fileno(), termios.TCSADRAIN, new_attrs)
self.usage()
@staticmethod
def usage():
"""Print usage statement."""
print('wasd for half speed, WASD for full speed.')
print('Space to stop.')
@staticmethod
def get_keypress():
"""Get the next available keypress from stdin."""
select.select([sys.stdin], [], [], 0)
return sys.stdin.read(1)
def callback(self, event):
"""Run the joystick event loop body."""
key = self.get_keypress()
if key in self.keymap:
left, right = self.keymap[key]
# Make 1 modifier go half speed.
left *= 50
right *= 50
# Add padding so that (100, 100) gets overwritten by (0, 0)
sys.stdout.write('Driving (L, R): ({}, {}) \r'.format(
left, right))
sys.stdout.flush()
msg = Int32()
msg.data = left
self.left_publisher.publish(msg)
msg.data = right
self.right_publisher.publish(msg)
def stop(self):
"""Handle shutdown signals."""
# You will regret the day that this fails.
termios.tcsetattr(sys.stdin.fileno(), termios.TCSADRAIN, self.settings)
# Shut off the wheels.
msg = Int32()
msg.data = 0
self.left_publisher.publish(msg)
self.right_publisher.publish(msg)
super(Joystick, self).stop()
def image_handler(self, compressed):
"""Handle each compressed video frame.
:param compressed: The compressed video frame.
:type compressed: sensor_msgs.msg.CompressedImage
"""
try:
# Decompress the message into an openCV frame.
bgr_frame = self.bridge.compressed_imgmsg_to_cv2(
compressed, 'bgr8')
except CvBridgeError as e:
print(e)
cv2.namedWindow('Joystick', cv2.WINDOW_NORMAL)
cv2.imshow('Joystick', bgr_frame)
cv2.waitKey(10)
class Controller():
def __init__(self,name):
self.topic_root = '/' + name + '/'
# Subscribe to sensors
#self.sensors_sub = rospy.Subscriber(self.topic_root + "sensors/package", miro.msg.sensors_package, self.sensors_callback)
self.cam_left_sub = rospy.Subscriber(
self.topic_root + "sensors/caml/compressed", CompressedImage, self.cam_left_callback)
self.cam_right_sub = rospy.Subscriber(
self.topic_root + "sensors/camr/compressed", CompressedImage, self.cam_right_callback)
# Arrays to hold image topics
self.cam_left_image = None
self.cam_right_image = None
# Create object to convert ROS images to OpenCV format
self.image_converter = CvBridge()
# Create resource for controlling body_node
self.pars = miro.utils.PlatformPars()
self.cam_model = miro.utils.CameraModel()
self.frame_w = 0
self.frame_h = 0
print("init")
def loop(self):
while True:
# state
time.sleep(0.1)
# b,g,r = cv2.split(self.cam_left_image)
# bgr_colour = np.uint8([[[b,g,r]]])
hsvl = cv2.cvtColor(self.cam_left_image, cv2.COLOR_BGR2HSV)
im_h = np.size(hsvl, 0)
im_w = np.size(hsvl, 1)
im_centre_h = im_h / 2.0
im_centre_w = im_w / 2.0
time.sleep(0.1)
outputl = self.cam_left_image.copy()
time.sleep(0.1)
outputr = self.cam_right_image.copy()
cv2.line(outputl, (0, int(round(im_centre_h))), (im_w, int(round(im_centre_h))), (100, 100, 100), 1)
cv2.line(outputl, (int(round(im_centre_w)), 0), (int(round(im_centre_w)), im_h), (100, 100, 100), 1)
#extract boundaries for masking image
target_hue = hsvl[0,0][0]
lower_bound = np.array([target_hue-20, 70, 70])
upper_bound = np.array([target_hue+20, 255, 255])
#mask image
mask = cv2.inRange(hsvl, lower_bound, upper_bound)
seg = mask
# Do some processing
seg = cv2.GaussianBlur(seg, (11,11), 0)
seg = cv2.erode(seg, None, iterations=2)
seg = cv2.dilate(seg, None, iterations=2)
# get circles
circles = cv2.HoughCircles(seg, cv2.HOUGH_GRADIENT, 1, 40, param1=10, param2=20,minRadius=0, maxRadius=0)
# Get largest circle
max_circle = None
max_circle_norm = [None, None, None]
if circles is not None:
self.max_rad = 0
circles = np.uint16(np.around(circles))
for c in circles[0,:]:
cv2.circle(seg, (c[0], c[1]), c[2], (0, 255, 0), 2)
if c[2] > self.max_rad:
self.max_rad = c[2]
max_circle = c
max_circle_norm[0] = int(round(((max_circle[0] - im_centre_w) / im_centre_w) * 100.0))
max_circle_norm[1] = int(round(-((max_circle[1] - im_centre_h) / im_centre_h) * 100.0))
max_circle_norm[2] = int(round((max_circle[2]/im_centre_w)*100.0))
#Debug Only
cv2.circle(outputl, (max_circle[0], max_circle[1]), max_circle[2], (0, 255, 0), 2)
cv2.circle(outputl, (max_circle[0], max_circle[1]), 1, (0, 255, 0), 2)
location_str = "x: " + str(max_circle_norm[0]) + "," + "y: " + str(max_circle_norm[1]) + "," + "r: " + str(max_circle[2])
text_y_offset = 18
for i, line in enumerate(location_str.split(",")):
text_y = max_circle[1] - text_y_offset + i*text_y_offset
cv2.putText(outputl, line, (max_circle[0]+5, text_y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 3)
cv2.putText(outputl, line, (max_circle[0]+5, text_y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
else:
pass
#debug_image = self.image_converter.cv2_to_imgmsg(outputl, "bgr8")
time.sleep(0.1)
cv2.imshow("left", outputl)
#time.sleep(1)
#cv2.imshow("debug_image", debug_image)
print(max_circle_norm)
# cv2.imshow("left", hsvl)
# time.sleep(1)
# hsvr = cv2.cvtColor(self.cam_right_image, cv2.COLOR_BGR2HSV)
# cv2.imshow("right", hsvr)
if cv2.waitKey(1)&0xff == 27: # esc键
break
cv2.destroyAllWindows()
def cam_left_callback(self, ros_image):
try:
self.cam_left_image = self.image_converter.compressed_imgmsg_to_cv2(ros_image, "bgr8")
im_h, im_w = self.cam_left_image.shape[:2]
if self.frame_w != im_w and self.frame_h != im_h:
self.frame_w, self.frame_h = im_w, im_h
self.cam_model.set_frame_size(self.frame_w, self.frame_h)
except CvBridgeError as e:
print("Conversion of left image failed \n")
print(e)
def cam_right_callback(self, ros_image):
try:
self.cam_right_image = self.image_converter.compressed_imgmsg_to_cv2(ros_image, "bgr8")
im_h, im_w = self.cam_right_image.shape[:2]
if self.frame_w != im_w and self.frame_h != im_h:
self.frame_w, self.frame_h = im_w, im_h
self.cam_model.set_frame_size(self.frame_w, self.frame_h)
except CvBridgeError as e:
print("Conversion of right image failed \n")
print(e)
def callback(self, ros_data):
bridge = CvBridge()
self.image = bridge.compressed_imgmsg_to_cv2(
ros_data, desired_encoding='passthrough')
class Detector:
def __init__(self):
rospy.init_node('turtlebot_detector', anonymous=True)
self.bridge = CvBridge()
self.detected_objects_pub = rospy.Publisher('/detector/objects', DetectedObjectList, queue_size=10)
if USE_TF:
self.detection_graph = tf.Graph()
with self.detection_graph.as_default():
od_graph_def = tf.GraphDef()
with tf.gfile.GFile(PATH_TO_MODEL, 'rb') as fid:
serialized_graph = fid.read()
od_graph_def.ParseFromString(serialized_graph)
tf.import_graph_def(od_graph_def,name='')
self.image_tensor = self.detection_graph.get_tensor_by_name('image_tensor:0')
self.d_boxes = self.detection_graph.get_tensor_by_name('detection_boxes:0')
self.d_scores = self.detection_graph.get_tensor_by_name('detection_scores:0')
self.d_classes = self.detection_graph.get_tensor_by_name('detection_classes:0')
self.num_d = self.detection_graph.get_tensor_by_name('num_detections:0')
config = tf.ConfigProto()
config.gpu_options.allow_growth = True
self.sess = tf.Session(graph=self.detection_graph, config=config)
# self.sess = tf.Session(graph=self.detection_graph)
# camera and laser parameters that get updated
self.cx = 0.
self.cy = 0.
self.fx = 1.
self.fy = 1.
self.laser_ranges = []
self.laser_angle_increment = 0.01 # this gets updated
self.object_publishers = {}
self.object_labels = load_object_labels(PATH_TO_LABELS)
self.tf_listener = TransformListener()
#rospy.Subscriber('/raspicam_node/image_raw', Image, self.camera_callback, queue_size=1, buff_size=2**24)
#rospy.Subscriber('/raspicam_node/image/compressed', CompressedImage, self.compressed_camera_callback, queue_size=1, buff_size=2**24)
#rospy.Subscriber('/raspicam_node/camera_info', CameraInfo, self.camera_info_callback)
rospy.Subscriber('/camera/image_raw', Image, self.camera_callback, queue_size=1, buff_size=2**24)
rospy.Subscriber('/camera/image/compressed', CompressedImage, self.compressed_camera_callback, queue_size=1, buff_size=2**24)
rospy.Subscriber('/camera/camera_info', CameraInfo, self.camera_info_callback)
rospy.Subscriber('/scan', LaserScan, self.laser_callback)
def run_detection(self, img):
""" runs a detection method in a given image """
image_np = self.load_image_into_numpy_array(img)
image_np_expanded = np.expand_dims(image_np, axis=0)
if USE_TF:
# uses MobileNet to detect objects in images
# this works well in the real world, but requires
# good computational resources
with self.detection_graph.as_default():
(boxes, scores, classes, num) = self.sess.run(
[self.d_boxes,self.d_scores,self.d_classes,self.num_d],
feed_dict={self.image_tensor: image_np_expanded})
return self.filter(boxes[0], scores[0], classes[0], num[0])
else:
# uses a simple color threshold to detect stop signs
# this will not work in the real world, but works well in Gazebo
# with only stop signs in the environment
R = image_np[:,:,0].astype(np.int) > image_np[:,:,1].astype(np.int) + image_np[:,:,2].astype(np.int)
Ry, Rx, = np.where(R)
if len(Ry)>0 and len(Rx)>0:
xmin, xmax = Rx.min(), Rx.max()
ymin, ymax = Ry.min(), Ry.max()
boxes = [[float(ymin)/image_np.shape[1], float(xmin)/image_np.shape[0], float(ymax)/image_np.shape[1], float(xmax)/image_np.shape[0]]]
scores = [.99]
classes = [13]
num = 1
else:
boxes = []
scores = 0
classes = 0
num = 0
return boxes, scores, classes, num
def filter(self, boxes, scores, classes, num):
""" removes any detected object below MIN_SCORE confidence """
f_scores, f_boxes, f_classes = [], [], []
f_num = 0
for i in range(num):
if scores[i] >= MIN_SCORE:
f_scores.append(scores[i])
f_boxes.append(boxes[i])
f_classes.append(int(classes[i]))
f_num += 1
else:
break
return f_boxes, f_scores, f_classes, f_num
def load_image_into_numpy_array(self, img):
""" converts opencv image into a numpy array """
(im_height, im_width, im_chan) = img.shape
return np.array(img.data).reshape((im_height, im_width, 3)).astype(np.uint8)
def project_pixel_to_ray(self,u,v):
""" takes in a pixel coordinate (u,v) and returns a tuple (x,y,z)
that is a unit vector in the direction of the pixel, in the camera frame.
This function access self.fx, self.fy, self.cx and self.cy """
x = (u - self.cx)/self.fx
y = (v - self.cy)/self.fy
norm = math.sqrt(x*x + y*y + 1)
x /= norm
y /= norm
z = 1.0 / norm
return (x,y,z)
def estimate_distance(self, thetaleft, thetaright, ranges):
""" estimates the distance of an object in between two angles
using lidar measurements """
leftray_indx = min(max(0,int(thetaleft/self.laser_angle_increment)),len(ranges))
rightray_indx = min(max(0,int(thetaright/self.laser_angle_increment)),len(ranges))
if leftray_indx<rightray_indx:
meas = ranges[rightray_indx:] + ranges[:leftray_indx]
else:
meas = ranges[rightray_indx:leftray_indx]
num_m, dist = 0, 0
for m in meas:
if m>0 and m<float('Inf'):
dist += m
num_m += 1
if num_m>0:
dist /= num_m
return dist
def camera_callback(self, msg):
""" callback for camera images """
# save the corresponding laser scan
img_laser_ranges = list(self.laser_ranges)
try:
img = self.bridge.imgmsg_to_cv2(msg, "passthrough")
img_bgr8 = self.bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
print(e)
self.camera_common(img_laser_ranges, img, img_bgr8)
def compressed_camera_callback(self, msg):
""" callback for camera images """
# save the corresponding laser scan
img_laser_ranges = list(self.laser_ranges)
try:
img = self.bridge.compressed_imgmsg_to_cv2(msg, "passthrough")
img_bgr8 = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
print(e)
self.camera_common(img_laser_ranges, img, img_bgr8)
def camera_common(self, img_laser_ranges, img, img_bgr8):
(img_h,img_w,img_c) = img.shape
# runs object detection in the image
(boxes, scores, classes, num) = self.run_detection(img)
if num > 0:
# create list of detected objects
detected_objects = DetectedObjectList()
print "detected_objects:", detected_objects
# some objects were detected
for (box,sc,cl) in zip(boxes, scores, classes):
ymin = int(box[0]*img_h)
xmin = int(box[1]*img_w)
ymax = int(box[2]*img_h)
xmax = int(box[3]*img_w)
xcen = int(0.5*(xmax-xmin)+xmin)
ycen = int(0.5*(ymax-ymin)+ymin)
cv2.rectangle(img_bgr8, (xmin,ymin), (xmax,ymax), (255,0,0), 2)
# computes the vectors in camera frame corresponding to each sides of the box
rayleft = self.project_pixel_to_ray(xmin,ycen)
rayright = self.project_pixel_to_ray(xmax,ycen)
# convert the rays to angles (with 0 poiting forward for the robot)
thetaleft = math.atan2(-rayleft[0],rayleft[2])
thetaright = math.atan2(-rayright[0],rayright[2])
if thetaleft<0:
thetaleft += 2.*math.pi
if thetaright<0:
thetaright += 2.*math.pi
# estimate the corresponding distance using the lidar
dist = self.estimate_distance(thetaleft,thetaright,img_laser_ranges)
if not self.object_publishers.has_key(cl):
self.object_publishers[cl] = rospy.Publisher('/detector/'+self.object_labels[cl],
DetectedObject, queue_size=10)
# publishes the detected object and its location
object_msg = DetectedObject()
object_msg.id = cl
object_msg.name = self.object_labels[cl]
object_msg.confidence = sc
object_msg.distance = dist
object_msg.thetaleft = thetaleft
object_msg.thetaright = thetaright
object_msg.corners = [ymin,xmin,ymax,xmax]
self.object_publishers[cl].publish(object_msg)
# add detected object to detected objects list
detected_objects.objects.append(self.object_labels[cl])
detected_objects.ob_msgs.append(object_msg)
self.detected_objects_pub.publish(detected_objects)
# displays the camera image
#print "detected_objects num:", num
cv2.imshow("Camera", img_bgr8)
cv2.waitKey(1)
def camera_info_callback(self, msg):
""" extracts relevant camera intrinsic parameters from the camera_info message.
cx, cy are the center of the image in pixel (the principal point), fx and fy are
the focal lengths. Stores the result in the class itself as self.cx, self.cy,
self.fx and self.fy """
self.cx = msg.P[2]
self.cy = msg.P[6]
self.fx = msg.P[0]
self.fy = msg.P[5]
def laser_callback(self, msg):
""" callback for thr laser rangefinder """
self.laser_ranges = msg.ranges
self.laser_angle_increment = msg.angle_increment
def run(self):
rospy.spin()
def panorama():
stitcher = Stitcher()
br = CvBridge()
rospy.init_node('panorama', anonymous=True)
# Load parameters
input_image_topic = "camera/compressed"
if rospy.has_param('~input_image_topic'):
input_image_topic = rospy.get_param('~input_image_topic')
input_image_topic = '%s/compressed' % input_image_topic
else:
rospy.logwarn(
"input image topic not provided on param 'input_image_topic'; using %s"
% input_image_topic)
output_image_topic = "panorama/compressed"
if rospy.has_param('~output_image_topic'):
output_image_topic = rospy.get_param('~output_image_topic')
output_image_topic = '%s/compressed' % output_image_topic
else:
rospy.logwarn(
"output image topic not provided on param 'output_image_topic'; using %s"
% output_image_topic)
num_segments = 2
if rospy.has_param('~num_segments'):
num_segments = rospy.get_param('~num_segments')
else:
rospy.logwarn(
"panorama number of segments not provided on param 'num_segments'; using %s"
% num_segments)
# Create publisher
panorama_publisher = rospy.Publisher(output_image_topic,
CompressedImage,
queue_size=1)
# Create panorama
rospy.loginfo("Waiting for image on topic %s" % input_image_topic)
imageA = rospy.wait_for_message(input_image_topic, CompressedImage)
imageA = br.compressed_imgmsg_to_cv2(imageA)
for n in range(1, num_segments):
# move
rospy.loginfo("Waiting for image on topic %s" % input_image_topic)
imageB = rospy.wait_for_message(input_image_topic, CompressedImage)
imageB = br.compressed_imgmsg_to_cv2(imageB)
rospy.loginfo("Stitching...")
(result, vis) = stitcher.stitch([imageA, imageB], showMatches=True)
imageA = result
# Show the images
cv2.imshow("Image A", imageA)
cv2.imshow("Image B", imageB)
cv2.imshow("Keypoint Matches", vis)
cv2.imshow("Result", result)
cv2.waitKey(0)
# Output panorama to file
cv2.imwrite("output.jpg", result)
# Publish panorama to ROS
compressed_img_message = br.cv2_to_compressed_imgmsg(result)
rospy.loginfo("Publishing panorama to topic %s" % output_image_topic)
panorama_publisher.publish(compressed_img_message)
class AntiInstagramNode(DTROS):
"""
Subscriber:
~uncorrected_image/compressed: The uncompressed image coming from the camera
Publisher:
~
"""
def __init__(self, node_name):
super(AntiInstagramNode, self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Read parameters
self._interval = rospy.get_param("~interval")
self._color_balance_percentage = rospy.get_param(
"~color_balance_scale")
self._output_scale = rospy.get_param("~output_scale")
# Construct publisher
self.pub = rospy.Publisher("~thresholds",
AntiInstagramThresholds,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
# Construct subscriber
self.uncorrected_image_subscriber = rospy.Subscriber(
'~uncorrected_image/compressed',
CompressedImage,
self.store_image_msg,
buff_size=10000000,
queue_size=1)
# Initialize Timer
rospy.Timer(rospy.Duration(self._interval),
self.calculate_new_parameters)
# Initialize objects and data
self.ai = AntiInstagram()
self.bridge = CvBridge()
self.image_msg = None
self.mutex = Lock()
# ---
self.log("Initialized.")
def store_image_msg(self, image_msg):
with self.mutex:
self.image_msg = image_msg
def decode_image_msg(self):
with self.mutex:
try:
image = self.bridge.compressed_imgmsg_to_cv2(
self.image_msg, "bgr8")
except ValueError as e:
self.log('Anti_instagram cannot decode image: %s' % e)
return image
def calculate_new_parameters(self, event):
if self.image_msg is None:
self.log("[%s] Waiting for first image!")
return
image = self.decode_image_msg()
(lower_thresholds,
higher_thresholds) = self.ai.calculate_color_balance_thresholds(
image, self._output_scale, self._color_balance_percentage)
# Publish parameters
msg = AntiInstagramThresholds()
msg.low = lower_thresholds
msg.high = higher_thresholds
self.pub.publish(msg)
class ObjectDetector:
"""
A very naive object detector that detects obstacles and goal posts based on their color.
Detection Algorithm
-------------------
A simple filtering pipeline is used to detect the objects:
1.) Remove parts of the image based on color limits for the hsv channels. After removal only the objects should remain
2.) Convert Image to black & white
3.) Find contours on the image to detect object borders
4.) Calculate center, width and height based on the contours
Parameters
----------
The color limits can be configured using the ROS parameter server.
See cfg/image_processing.cfg for details on all available parameters.
The color limits depend on the lighting conditions in the room. I.e. they have to be adapted before the competition
The debug parameter enables/disables the output of additional images that can be used for parameter tuning.
Subscribed Topics
-----------------
input_obstacles_image - Camera image (raw or compressed - user needs to ensure XOR!)
Published Topics
----------------
/obstacles - A list of currently detected obstacles
/goals - A list of currently detected goal posts
/debug_image_result - Visualization of the detected contours
/debug_image_goals_left - Visualization of the color segmentation for the left goal post
/debug_image_goals_right - Visualization of the color segmentation for the right goal post
/debug_image_obstacles - Visualization of the color segmentation for the obstacle detection
"""
def __init__(self):
# bridge converts ros images <-> opencv images
self.bridge = CvBridge()
# parameters
self.obstacle_color_limits = ColorLimit()
self.left_goal_limits = ColorLimit()
self.right_goal_limits = ColorLimit()
self.binarization_threshold = 30
self.debug_mode = False
# pubs/subs
self.image_sub = rospy.Subscriber("input_obstacles_image", Image,
self.callback)
self.comp_image_sub = rospy.Subscriber(
"input_obstacles_image_compressed", CompressedImage,
self.callback_compressed)
self.obstacle_publisher = rospy.Publisher("/obstacles",
Obstacles,
queue_size=5)
self.goal_publisher = rospy.Publisher("/goals", Goals, queue_size=5)
self.debug_result_image_pub = rospy.Publisher("/debug_image_result",
Image,
queue_size=5)
self.debug_image_goals_left_pub = rospy.Publisher(
"/debug_image_goals_left", Image, queue_size=5)
self.debug_image_goals_right_pub = rospy.Publisher(
"/debug_image_goals_right", Image, queue_size=5)
self.debug_image_obstacles_pub = rospy.Publisher(
"/debug_image_obstacles", Image, queue_size=5)
rospy.sleep(0.2)
def detect_contours(self,
bgr_image,
hsv_image,
low_limit,
high_limit,
debug_image_publisher=None):
# ============= YOUR CODE GOES HERE! =====
# hint: use opencv to find contours
# use other cv2.? methods
# use cv2.findContours(...) - method
# ============= YOUR CODE ENDS HERE! =====
return contours
def detect_obstacles(self, bgr_image, hsv_image):
"""
:return: contours
"""
low_col = self.obstacle_color_limits.low
high_col = self.obstacle_color_limits.high
debug_pub = None
if self.debug_mode:
debug_pub = self.debug_image_obstacles_pub
return self.detect_contours(bgr_image, hsv_image, low_col, high_col,
debug_pub)
def detect_goals(self, bgr_image, hsv_image):
debug_pub_left = None
debug_pub_right = None
if self.debug_mode:
debug_pub_left = self.debug_image_goals_left_pub
debug_pub_right = self.debug_image_goals_right_pub
low_col = self.left_goal_limits.low
high_col = self.left_goal_limits.high
left_contours = self.detect_contours(bgr_image, hsv_image,
self.left_goal_limits.low,
self.left_goal_limits.high,
debug_pub_left)
right_contours = self.detect_contours(bgr_image, hsv_image,
self.right_goal_limits.low,
self.right_goal_limits.high,
debug_pub_right)
return left_contours, right_contours
def contour_to_object(self, contour):
"""
:return: Obstacle from contour
"""
ob = Object()
moment = cv2.moments(contour)
if moment['m00'] != 0:
ob.x = int(moment['m10'] / moment['m00'])
ob.y = int(moment['m01'] / moment['m00'])
else:
raise RuntimeError("Contour Moment Zero")
min_x = sys.maxsize
max_x = 0
min_y = sys.maxsize
max_y = 0
for p in contour:
x, y = p[0]
min_x = min(min_x, x)
max_x = max(max_x, x)
min_y = min(min_y, y)
max_y = max(max_y, y)
ob.width = max_x - min_x
ob.height = max_y - min_y
return ob
def callback_compressed(self, compressed_image):
'''
callback for compressed images
NOTE: do not use both at same time : (callback_compressed & callback)
'''
try:
bgr_image = self.bridge.compressed_imgmsg_to_cv2(
compressed_image, desired_encoding="bgr8")
self.process_bgr_image(bgr_image)
except CvBridgeError as e:
rospy.logerr(e)
def callback(self, raw_image):
'''
callback for raw images
NOTE: do not use both at same time : (callback_compressed & callback)
'''
try:
bgr_image = self.bridge.imgmsg_to_cv2(raw_image,
desired_encoding="bgr8")
self.process_bgr_image(bgr_image)
except CvBridgeError as e:
rospy.logerr(e)
def process_bgr_image(self, bgr_image):
try:
hsv_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2HSV)
obstacle_contours = self.detect_obstacles(bgr_image, hsv_image)
left_goal_contours, right_goal_contours = self.detect_goals(
bgr_image, hsv_image)
obstacles = Obstacles()
for c in obstacle_contours:
try:
obstacle = self.contour_to_object(c)
obstacles.Obstacles.append(obstacle)
except RuntimeError as e:
pass
goals = Goals()
for c in left_goal_contours:
try:
goals.left_goals.append(self.contour_to_object(c))
except RuntimeError:
pass
for c in right_goal_contours:
try:
goals.right_goals.append(self.contour_to_object(c))
except RuntimeError:
pass
self.obstacle_publisher.publish(obstacles)
self.goal_publisher.publish(goals)
if self.debug_mode:
result_img = bgr_image.copy()
cv2.drawContours(result_img, obstacle_contours, -1,
(0, 255, 0), 3)
cv2.drawContours(result_img, left_goal_contours, -1,
(255, 0, 0), 3)
cv2.drawContours(result_img, right_goal_contours, -1,
(0, 0, 255), 3)
self.debug_result_image_pub.publish(
self.bridge.cv2_to_imgmsg(result_img, "bgr8"))
except CvBridgeError as e:
rospy.loginfo(e)
def reconfigure_callback(self, pdic, level):
'''
every time a parameter changes through dynamic reconfigure this callback gets called
and all parameters are provided as a dictionary (pdic)
'''
self.obstacle_color_limits.low = (pdic['obs_h_lo'], pdic['obs_s_lo'],
pdic['obs_v_lo'])
self.obstacle_color_limits.high = (pdic['obs_h_hi'], pdic['obs_s_hi'],
pdic['obs_v_hi'])
self.left_goal_limits.low = (pdic['goal_left_h_lo'],
pdic['goal_left_s_lo'],
pdic['goal_left_v_lo'])
self.left_goal_limits.high = (pdic['goal_left_h_hi'],
pdic['goal_left_s_hi'],
pdic['goal_left_v_hi'])
self.right_goal_limits.low = (pdic['goal_right_h_lo'],
pdic['goal_right_s_lo'],
pdic['goal_right_v_lo'])
self.right_goal_limits.high = (pdic['goal_right_h_hi'],
pdic['goal_right_s_hi'],
pdic['goal_right_v_hi'])
self.binarization_threshold = pdic['binarization_threshold']
self.debug_mode = pdic['debug']
# return a possibly updated configuration (mandatory)
return pdic
def start(self):
# Enable dynamic reconfigure parameters
srv = Server(rdl_rgb_obstacle_detectionConfig,
callback=self.reconfigure_callback)
# wait for ctrl + c to be pressed (avoid killing process so that callbacks can do their work)
rospy.spin()
# user has pressed ctrl + c, shut down safely
rospy.loginfo("Shutting down")
cv2.destroyAllWindows()
class ARNode(DTROS):
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ARNode, self).__init__(node_name=node_name,node_type=NodeType.GENERIC)
self.veh = rospy.get_namespace().strip("/")
rospack = rospkg.RosPack()
# Initialize an instance of Renderer giving the model in input.
self.renderer = Renderer(rospack.get_path('augmented_reality_apriltag') + '/src/models/duckie.obj')
#
# Write your code here
#
self.bridge = CvBridge()
self.image = None
self.image_height = None
self.image_width = None
self.cam_info = None
self.camera_info_received = False
self.at_detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
self.sub_image_comp = rospy.Subscriber(
f'/{self.veh}/camera_node/image/compressed',
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1
)
self.sub_cam_info = rospy.Subscriber(f'/{self.veh}/camera_node/camera_info', CameraInfo,
self.cb_camera_info, queue_size=1)
self.pub_aug_image = rospy.Publisher(f'/{self.veh}/{node_name}/image/compressed',
CompressedImage, queue_size=10)
def image_cb(self, image_msg):
try:
image = self.readImage(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
self.image = image
self.image_height = np.shape(image)[0]
self.image_width = np.shape(image)[1]
def cb_camera_info(self, msg):
if not self.camera_info_received:
self.cam_info = msg
self.camera_info_received = True
def detect_april_tag(self):
K = np.array(self.cam_info.K).reshape((3, 3))
camera_params = (K[0, 0], K[1, 1], K[0, 2], K[1, 2])
img_grey = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY)
tags = self.at_detector.detect(img_grey, True, camera_params, 0.065)
list_hom_tag = []
for tag in tags:
list_hom_tag.append(tag.homography)
return list_hom_tag
def projection_matrix(self, intrinsic, homography):
"""
Write here the compuatation for the projection matrix, namely the matrix
that maps the camera reference frame to the AprilTag reference frame.
"""
K = intrinsic
P_ext = np.linalg.inv(K) @ homography
r_1 = P_ext[:, 0]
r_1_norm = np.linalg.norm(r_1)
r_1 = r_1 / r_1_norm
r_2 = P_ext[:, 1]
r_2_norm = np.linalg.norm(r_2)
r_2 = r_2 / r_2_norm
r_3 = np.cross(r_1, r_2)
t_norm = (r_1_norm + r_2_norm)/2
t = P_ext[:, 2] / t_norm
P_ext_new = np.concatenate((r_1, r_2, r_3, t)).reshape((-1, 4), order='F')
P = K @ P_ext_new
return P
def readImage(self, msg_image):
"""
Convert images to OpenCV images
Args:
msg_image (:obj:`CompressedImage`) the image from the camera node
Returns:
OpenCV image
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg_image)
return cv_image
except CvBridgeError as e:
self.log(e)
return []
def readYamlFile(self,fname):
"""
Reads the 'fname' yaml file and returns a dictionary with its input.
You will find the calibration files you need in:
`/data/config/calibrations/`
"""
with open(fname, 'r') as in_file:
try:
yaml_dict = yaml.load(in_file)
return yaml_dict
except yaml.YAMLError as exc:
self.log("YAML syntax error. File: %s fname. Exc: %s"
%(fname, exc), type='fatal')
rospy.signal_shutdown()
return
def onShutdown(self):
super(ARNode, self).onShutdown()
def run(self):
while not rospy.is_shutdown():
if self.image is None:
continue
if not self.camera_info_received:
continue
list_hom_tag = self.detect_april_tag()
if not list_hom_tag:
continue
img_rend = self.image
for h in list_hom_tag:
proj_mat = self.projection_matrix(np.array(self.cam_info.K).reshape((3, 3)), h)
img_rend = self.renderer.render(img_rend, proj_mat)
comp_image = self.bridge.cv2_to_compressed_imgmsg(img_rend)
self.pub_aug_image.publish(comp_image)
class DetectorViz:
def __init__(self):
rospy.init_node('turtlebot_detector', anonymous=True)
self.bridge = CvBridge()
self.tf_listener = TransformListener()
self.last_box_time = rospy.get_rostime()
rospy.Subscriber('/detector/objects', DetectedObjectList, self.detected_objects_name_callback, queue_size=10)
self.detected_objects = None
rospy.Subscriber('/camera_relay/image_raw', Image, self.camera_callback, queue_size=1, buff_size=2**24)
rospy.Subscriber('/camera_relay/image/compressed', CompressedImage, self.compressed_camera_callback, queue_size=1, buff_size=2**24)
def load_image_into_numpy_array(self, img):
""" converts opencv image into a numpy array """
(im_height, im_width, im_chan) = img.shape
return np.array(img.data).reshape((im_height, im_width, 3)).astype(np.uint8)
def detected_objects_name_callback(self, msg):
rospy.loginfo("There are %i detected objects" % len(msg.objects))
rospy.loginfo("Object 1: %s" % str(msg.objects[0]))
self.detected_objects = msg
self.last_box_time = rospy.get_rostime()
def camera_callback(self, msg):
""" callback for camera images """
try:
img = self.bridge.imgmsg_to_cv2(msg, "passthrough")
img_bgr8 = self.bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
print(e)
if (msg.header.stamp.to_sec() - self.last_box_time.to_sec()) > BOX_TIMEOUT:
self.detected_objects = None
if np.abs(rospy.get_rostime().to_sec() - self.last_box_time.to_sec()) > BOX_TIMEOUT:
self.detected_objects = None
self.camera_common(img, img_bgr8)
def compressed_camera_callback(self, msg):
""" callback for camera images """
# save the corresponding laser scan
try:
img = self.bridge.compressed_imgmsg_to_cv2(msg, "passthrough")
img_bgr8 = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
print(e)
if np.abs(rospy.get_rostime().to_sec() - self.last_box_time.to_sec()) > BOX_TIMEOUT:
self.detected_objects = None
self.camera_common(img, img_bgr8)
def camera_common(self, img, img_bgr8):
(img_h,img_w,img_c) = img.shape
draw_color = (0,255,0)
if self.detected_objects is not None:
for ob_msg in self.detected_objects.ob_msgs:
ymin, xmin, ymax, xmax = [int(x) for x in ob_msg.corners]
cv2.rectangle(img_bgr8, (xmin,ymin), (xmax,ymax), draw_color, 2)
# cool add-on by student in 2018 class
cv2.putText(img_bgr8, ob_msg.name + ":" + str(round(ob_msg.confidence, 2)), (xmin, ymin+13), CV2_FONT, .5, draw_color)
cv2.imshow("Camera", img_bgr8)
cv2.waitKey(1)
def run(self):
rospy.spin()
# Create output directory
if not os.path.exists(output_path):
os.makedirs(output_path)
# Check if topic is in bag
info = bag.get_type_and_topic_info()
if topic not in info.topics:
raise RuntimeError("Opps! topic not in bag!")
# Check image message type
msg_type = info.topics[topic].msg_type
supported_msgs = ["sensor_msgs/CompressedImage", "sensor_msgs/Image"]
if msg_type not in supported_msgs:
err_msg = "bag2vid only supports %s!" % " or ".join(supported_msgs)
raise RuntimeError(err_msg)
# Convert bag to images
br = CvBridge()
index = 0
for topic, msg, t in bag.read_messages(topics=[topic]):
# Convert image message to np.array
if msg_type == "sensor_msgs/CompressedImage":
image = br.compressed_imgmsg_to_cv2(msg)
else:
image = br.imgmsg_to_cv2(msg)
# Write image to file
image_fname = join(output_path, "image_%d.jpg" % index)
cv2.imwrite(image_fname, image)
index += 1
import cv2
import yaml
import rospy
from cv_bridge import CvBridge
import numpy as np
bridge = CvBridge()
streamCam = file('camData.yaml', 'r')
dataCam = yaml.load(streamCam)
cv2_image = []
for data in dataCam:
cv2_image = bridge.compressed_imgmsg_to_cv2(data)
cv2.imshow('frame', cv2_image)
cv2.waitKey(50)
class ImageServer:
''' Image Server for iOS to ROS '''
def __init__(self):
''' Initialize the server to enable the collection and publication of image and camera data. '''
rospy.init_node('image_server')
self.port = rospy.get_param('~port_number')
self.camera_name = rospy.get_param('~camera_name')
self.ios_clock_offset = 0
self.bridge = CvBridge()
self.clock_sub = rospy.Subscriber('/ios_clock', Float64,
self.handle_ios_clock)
self.pub_camera = rospy.Publisher('/' + self.camera_name +
'/image_raw/compressed',
CompressedImage,
queue_size=10)
self.pub_lowres = rospy.Publisher('/' + self.camera_name +
'_lowres/image_raw/compressed',
CompressedImage,
queue_size=10)
self.pub_camera_info = rospy.Publisher('/' + self.camera_name +
'/camera_info',
CameraInfo,
queue_size=10)
self.pub_lowres_info = rospy.Publisher('/' + self.camera_name +
'_lowres/camera_info',
CameraInfo,
queue_size=10)
self.cvmsg = CompressedImage()
self.image_data = {}
self.msg = CompressedImage()
self.intrinsic_msg = CameraInfo()
self.updated_intrinsics = None
self.last_packet_timestamp = rospy.Time(0.0)
UDP_IP = "0.0.0.0"
self.sock = socket.socket(
socket.AF_INET, #Internet
socket.SOCK_DGRAM) #UDP
self.sock.bind((UDP_IP, self.port))
def handle_ios_clock(self, msg):
''' Get the offset between iOS time and ROS time from the clock Subscriber. '''
self.ios_clock_offset = msg.data
def complete_packet_assembly(self, image_number):
''' Finish assembling an image when all of its packets have been received. '''
self.image_data[image_number]['payload'].sort()
image = ''
for packet in self.image_data[image_number]['payload']:
image += packet[1]
self.msg.header.stamp = rospy.Time(
float(self.ios_clock_offset) +
float(self.image_data[image_number]['timestamp']))
self.msg.header.frame_id = self.camera_name
self.msg.data = image
self.msg.format = 'jpeg'
# Convert the iOS image to a cv2 image and lower the resolution
resize_factor = 1 / 3.
cv_image = self.bridge.compressed_imgmsg_to_cv2(self.msg)
flipped_image = cv2.transpose(
cv_image
) # Transpose and flip the image so it is aligned with the correct camera axes
flipped_image = cv2.flip(flipped_image, 1)
full_res = self.bridge.cv2_to_compressed_imgmsg(flipped_image)
self.msg.data = full_res.data
lower_res = cv2.resize(flipped_image, (0, 0),
fx=resize_factor,
fy=resize_factor)
image = self.bridge.cv2_to_compressed_imgmsg(lower_res)
self.cvmsg.data = image.data
self.cvmsg.format = image.format
self.cvmsg.header.stamp = rospy.Time(
float(self.ios_clock_offset) +
float(self.image_data[image_number]['timestamp']))
self.cvmsg.header.frame_id = self.camera_name
# Update the camera intrinsics for the flipped images
self.updated_intrinsics = CameraInfo()
self.updated_intrinsics.K = [
v * resize_factor if v != 1.0 else v
for v in self.image_data[image_number]['intrinsics_message'].K
]
self.updated_intrinsics.P = [
v * resize_factor if v != 1.0 else v
for v in self.image_data[image_number]['intrinsics_message'].P
]
self.updated_intrinsics.width = self.image_data[image_number][
'intrinsics_message'].width * resize_factor
self.updated_intrinsics.height = self.image_data[image_number][
'intrinsics_message'].height * resize_factor
self.updated_intrinsics.header.stamp = rospy.Time(
float(self.ios_clock_offset) +
float(self.image_data[image_number]['timestamp']))
self.updated_intrinsics.header.frame_id = self.camera_name
def run(self):
''' Publish image and camera intrinsics data. '''
while not rospy.is_shutdown():
data, addr = self.sock.recvfrom(1600)
packet_offset = 0
image_number, packet_number = struct.unpack(
'<BB', data[packet_offset:packet_offset + 2])
packet_offset += 2
# Check if the packet is the first in a given image
if packet_number == 0:
total_packets = struct.unpack('<B', data[packet_offset])[0]
packet_offset += 1
self.image_data[image_number] = {}
self.image_data[image_number][
'packets_expected'] = total_packets
self.image_data[image_number]['packets_received'] = 1
time_bytes = struct.unpack('<B', data[packet_offset])[0]
packet_offset += 1
intrinsic_bytes = struct.unpack('<B', data[packet_offset])[0]
packet_offset += 1
stampedTime = data[
packet_offset:packet_offset +
time_bytes] # TODO: remove magic numbers of 5 and 4
intrinsic_data = data[packet_offset +
time_bytes:packet_offset + time_bytes +
intrinsic_bytes]
intrinsics_vals = [float(x) for x in intrinsic_data.split(',')]
self.image_data[image_number]['timestamp'] = stampedTime
self.image_data[image_number]['payload'] = [
(packet_number,
data[packet_offset + time_bytes + intrinsic_bytes:])
]
# Set the camera intrinsics for each image
# The camera images are always transmitted from iOS in a landscape format, even when the camera is in portrait mode. Thus,we will utilize cv2 to flip and transpose the image
self.intrinsic_msg = CameraInfo(
header=Header(stamp=rospy.Time(
float(self.ios_clock_offset) + float(stampedTime))),
width=intrinsics_vals[
6], # Preemptively swap the width and the height since the image will be flipped
height=intrinsics_vals[5],
distortion_model='plumb_bob',
D=[
0, 0, 0, 0, 0
], #The iPhone automatically adjusts the image to remove distortion.
K=[
intrinsics_vals[0],
0,
intrinsics_vals[
3], # Preemptively switch the principal point offsets since the image will be transposed and flipped
0,
intrinsics_vals[1],
intrinsics_vals[2],
0,
0,
1
], # vals out of order because iOS uses column-major order
P=[
intrinsics_vals[0], 0, intrinsics_vals[3], 0, 0,
intrinsics_vals[1], intrinsics_vals[2], 0, 0, 0, 1, 0
])
self.image_data[image_number][
'intrinsics_message'] = self.intrinsic_msg
# Check if a partially completed image exists
elif image_number in self.image_data.keys():
self.image_data[image_number]['packets_received'] += 1
self.image_data[image_number]['payload'] += [
(packet_number, data[packet_offset:])
]
# Check if all of the packets for an image have been received
if self.image_data[image_number][
'packets_received'] == self.image_data[image_number][
'packets_expected']:
self.complete_packet_assembly(image_number)
# Ensure images are not published if an image with a later timestamp has already been published
if self.last_packet_timestamp == 0.0 or self.last_packet_timestamp < self.msg.header.stamp:
self.pub_camera.publish(self.msg)
self.pub_camera_info.publish(
self.image_data[image_number]
['intrinsics_message'])
self.pub_lowres.publish(self.cvmsg)
self.pub_lowres_info.publish(self.updated_intrinsics)
self.last_packet_timestamp = self.msg.header.stamp
class ThymioController:
def __init__(self):
"""Initialization."""
# initialize the node
rospy.init_node('thymio_controller' # name of the node
)
self.name = rospy.get_param('~robot_name')
# log robot name to console
rospy.loginfo('Controlling %s' % self.name)
# create velocity publisher
self.velocity_publisher = rospy.Publisher(
self.name + '/cmd_vel', # name of the topic
Twist, # message type
queue_size=10 # queue size
)
# create image subscriber
self.image_subscriber = rospy.Subscriber(
self.name + '/camera/image_raw/compressed', # name of the topic
CompressedImage, # message type
self.capture_image # function that hanldes incoming messages
)
# set node update frequency in Hz
self.rate = rospy.Rate(10)
self.photo = None
self.bridge = CvBridge()
def human_readable_pose2d(self, pose):
"""Converts pose message to a human readable pose tuple."""
# create a quaternion from the pose
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
# convert quaternion rotation to euler rotation
roll, pitch, yaw = euler_from_quaternion(quaternion)
result = (
pose.position.x, # x position
pose.position.y, # y position
yaw # theta angle
)
return result
def move_ahead(self, vel=0.2):
return Twist(
linear=Vector3(
vel, # moves forward .2 m/s
.0,
.0,
),
angular=Vector3(
.0,
.0,
.0,
))
def capture_image(self, data):
self.photo = data
def transform_image(self, img):
flat = img[:, :, 0]
return np.where(flat < 200, 0, 1)
def run(self):
#speed = input("photo sec:")
i = 0
while i < 10:
velocity = self.move_ahead(0)
self.velocity_publisher.publish(velocity)
i += 1
self.rate.sleep()
velocity = self.move_ahead(0)
self.velocity_publisher.publish(velocity)
speed = 10
i = 0
while i < speed:
i += 0.5
self.rate.sleep()
img = self.bridge.compressed_imgmsg_to_cv2(self.photo)
#img = self.transform_image(img)
print(type(img))
print(sum(sum(img)))
print(img.shape)
print(img)
plt.imsave(
'/home/usi/catkin_ws/src/landloc_control/src/binary_image.png',
img,
cmap=cm.gray)
