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)