def publish(self, event): if self.imgmsg is None: return now = rospy.Time.now() # setup ros message and publish with self.lock: self.imgmsg.header.stamp = now self.imgmsg.header.frame_id = self.frame_id self.pub.publish(self.imgmsg) if self.publish_info: info = CameraInfo() info.header.stamp = now info.header.frame_id = self.frame_id info.width = self.imgmsg.width info.height = self.imgmsg.height if self.fovx is not None and self.fovy is not None: fx = self.imgmsg.width / 2.0 / \ np.tan(np.deg2rad(self.fovx / 2.0)) fy = self.imgmsg.height / 2.0 / \ np.tan(np.deg2rad(self.fovy / 2.0)) cx = self.imgmsg.width / 2.0 cy = self.imgmsg.height / 2.0 info.K = np.array([fx, 0, cx, 0, fy, cy, 0, 0, 1.0]) info.P = np.array([fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0]) info.R = [1, 0, 0, 0, 1, 0, 0, 0, 1] self.pub_info.publish(info)
def __init__(self): rospy.init_node('image_publish') name = sys.argv[1] image = cv2.imread(name) #cv2.imshow("im", image) #cv2.waitKey(5) hz = rospy.get_param("~rate", 1) frame_id = rospy.get_param("~frame_id", "map") use_image = rospy.get_param("~use_image", True) rate = rospy.Rate(hz) self.ci_in = None ci_sub = rospy.Subscriber('camera_info_in', CameraInfo, self.camera_info_callback, queue_size=1) if use_image: pub = rospy.Publisher('image', Image, queue_size=1) ci_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=1) msg = Image() msg.header.stamp = rospy.Time.now() msg.header.frame_id = frame_id msg.encoding = 'bgr8' msg.height = image.shape[0] msg.width = image.shape[1] msg.step = image.shape[1] * 3 msg.data = image.tostring() if use_image: pub.publish(msg) ci = CameraInfo() ci.header = msg.header ci.height = msg.height ci.width = msg.width ci.distortion_model ="plumb_bob" # TODO(lucasw) need a way to set these values- have this node # subscribe to an input CameraInfo? ci.D = [0.0, 0.0, 0.0, 0, 0] ci.K = [500.0, 0.0, msg.width/2, 0.0, 500.0, msg.height/2, 0.0, 0.0, 1.0] ci.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] ci.P = [500.0, 0.0, msg.width/2, 0.0, 0.0, 500.0, msg.height/2, 0.0, 0.0, 0.0, 1.0, 0.0] # ci_pub.publish(ci) # TODO(lwalter) only run this is hz is positive, # otherwise wait for input trigger message to publish an image while not rospy.is_shutdown(): if self.ci_in is not None: ci = self.ci_in msg.header.stamp = rospy.Time.now() ci.header = msg.header if use_image: pub.publish(msg) ci_pub.publish(ci) if hz <= 0: rospy.sleep() else: rate.sleep()
def yaml_to_CameraInfo(calib_yaml): """ Parse a yaml file containing camera calibration data (as produced by rosrun camera_calibration cameracalibrator.py) into a sensor_msgs/CameraInfo msg. Parameters ---------- yaml_fname : str Path to yaml file containing camera calibration data Returns ------- camera_info_msg : sensor_msgs.msg.CameraInfo A sensor_msgs.msg.CameraInfo message containing the camera calibration data """ # Load data from file calib_data = yaml.load(calib_yaml) # Parse camera_info_msg = CameraInfo() camera_info_msg.width = calib_data["image_width"] camera_info_msg.height = calib_data["image_height"] camera_info_msg.K = calib_data["camera_matrix"]["data"] camera_info_msg.D = calib_data["distortion_coefficients"]["data"] camera_info_msg.R = calib_data["rectification_matrix"]["data"] camera_info_msg.P = calib_data["projection_matrix"]["data"] camera_info_msg.distortion_model = calib_data["distortion_model"] return camera_info_msg
def __init__(self): rospy.init_node('image_converter', anonymous=True) self.filtered_face_locations = rospy.get_param('camera_topic') self.image_pub = rospy.Publisher(self.filtered_face_locations,Image) self.image_info = rospy.Publisher('camera_info',CameraInfo) self.bridge = CvBridge() cap = cv2.VideoCapture('/home/icog-labs/Videos/video.mp4') print cap.get(5) path = rospkg.RosPack().get_path("robots_config") path = path + "/robot/camera.yaml" stream = file(path, 'r') calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] while (not rospy.is_shutdown()) and (cap.isOpened()): self.keystroke = cv2.waitKey(1000 / 30) ret, frame = cap.read() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # cv2.imshow('frame', gray) self.image_pub.publish(self.bridge.cv2_to_imgmsg(gray, "mono8")) self.image_info.publish(cam_info) if cv2.waitKey(1) & 0xFF == ord('q'): break
def send_test_messages(self, filename): self.msg_received = False # Publish the camera info TODO make this a field in the annotations file to dictate the source calibration file msg_info = CameraInfo() msg_info.height = 480 msg_info.width = 640 msg_info.distortion_model = "plumb_bob" msg_info.D = [-0.28048157543793056, 0.05674481026365553, -0.000988764087143394, -0.00026869128565781613, 0.0] msg_info.K = [315.128501, 0.0, 323.069638, 0.0, 320.096636, 218.012581, 0.0, 0.0, 1.0] msg_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg_info.P = [217.48876953125, 0.0, 321.3154072384932, 0.0, 0.0, 250.71084594726562, 202.30416165274983, 0.0, 0.0, 0.0, 1.0, 0.0] msg_info.roi.do_rectify = False # Publish the test image img = cv2.imread(filename) cvb = CvBridge() msg_raw = cvb.cv2_to_imgmsg(img, encoding="bgr8") self.pub_info.publish(msg_info) self.pub_raw.publish(msg_raw) # Wait for the message to be received timeout = rospy.Time.now() + rospy.Duration(10) # Wait at most 5 seconds for the node to reply while not self.msg_received and not rospy.is_shutdown() and rospy.Time.now() < timeout: rospy.sleep(0.1) self.assertLess(rospy.Time.now(), timeout, "Waiting for apriltag detection timed out.")
def publish( self ): # Get the image. image = self.__videoDeviceProxy.getImageRemote( self.__videoDeviceProxyName ); # Create Image message. ImageMessage = Image(); ImageMessage.header.stamp.secs = image[ 5 ]; ImageMessage.width = image[ 0 ]; ImageMessage.height = image[ 1 ]; ImageMessage.step = image[ 2 ] * image[ 0 ]; ImageMessage.is_bigendian = False; ImageMessage.encoding = 'bgr8'; ImageMessage.data = image[ 6 ]; self.__imagePublisher.publish( ImageMessage ); # Create CameraInfo message. # Data from the calibration phase is hard coded for now. CameraInfoMessage = CameraInfo(); CameraInfoMessage.header.stamp.secs = image[ 5 ]; CameraInfoMessage.width = image[ 0 ]; CameraInfoMessage.height = image[ 1 ]; CameraInfoMessage.D = [ -0.0769218451517258, 0.16183180613612602, 0.0011626049774280595, 0.0018733894100460534, 0.0 ]; CameraInfoMessage.K = [ 581.090096189648, 0.0, 341.0926325830606, 0.0, 583.0323248080421, 241.02441593704128, 0.0, 0.0, 1.0 ]; CameraInfoMessage.R = [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ]; CameraInfoMessage.P = [ 580.5918359588198, 0.0, 340.76398441334106, 0.0, 0.0, 582.4042541534321, 241.04182225157172, 0.0, 0.0, 0.0, 1.0, 0.0] ; CameraInfoMessage.distortion_model = 'plumb_bob'; #CameraInfoMessage.roi.x_offset = self.__ROIXOffset; #CameraInfoMessage.roi.y_offset = self.__ROIYOffset; #CameraInfoMessage.roi.width = self.__ROIWidth; #CameraInfoMessage.roi.height = self.__ROIHeight; #CameraInfoMessage.roi.do_rectify = self.__ROIDoRectify; self.__cameraInfoPublisher.publish( CameraInfoMessage );
def default(self, ci="unused"): if not self.component_instance.capturing: return # press [Space] key to enable capturing image_local = self.data["image"] image = Image() image.header = self.get_ros_header() image.header.frame_id += "/base_image" image.height = self.component_instance.image_height image.width = self.component_instance.image_width image.encoding = "rgba8" image.step = image.width * 4 # VideoTexture.ImageRender implements the buffer interface image.data = bytes(image_local) # fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = self.data["intrinsic_matrix"] camera_info = CameraInfo() camera_info.header = image.header camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = "plumb_bob" camera_info.K = [ intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], ] camera_info.R = R camera_info.P = [ intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx, intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty, intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0, ] self.publish(image) self.topic_camera_info.publish(camera_info)
def parse_yaml(self, filename): stream = file(filename, "r") calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data["image_width"] cam_info.height = calib_data["image_height"] cam_info.K = calib_data["camera_matrix"]["data"] cam_info.D = calib_data["distortion_coefficients"]["data"] cam_info.R = calib_data["rectification_matrix"]["data"] cam_info.P = calib_data["projection_matrix"]["data"] cam_info.distortion_model = calib_data["distortion_model"] return cam_info
def parse_yaml(filename): stream = file(filename, 'r') calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] return cam_info
def load_cam_info(calib_file): cam_info = CameraInfo() with open(calib_file,'r') as cam_calib_file : cam_calib = yaml.load(cam_calib_file) cam_info.height = cam_calib['image_height'] cam_info.width = cam_calib['image_width'] cam_info.K = cam_calib['camera_matrix']['data'] cam_info.D = cam_calib['distortion_coefficients']['data'] cam_info.R = cam_calib['rectification_matrix']['data'] cam_info.P = cam_calib['projection_matrix']['data'] cam_info.distortion_model = cam_calib['distortion_model'] return cam_info
def createMsg(self): msg = CameraInfo() msg.height = 480 msg.width = 640 msg.distortion_model = "plumb_bob" msg.D = [0.08199114285264993, -0.04549390835713936, -0.00040960290587863145, 0.0009833748346549968, 0.0] msg.K = [723.5609128875188, 0.0, 315.9845354772031, 0.0, 732.2615109685506, 242.26660681756633,\ 0.0, 0.0, 1.0] msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] msg.P = [737.1736450195312, 0.0, 316.0324931112791, 0.0, 0.0, 746.1573486328125, 241.59042622688867,\ 0.0, 0.0, 0.0, 1.0, 0.0] self.msg = msg
def yaml_to_CameraInfo(yaml_fname): with open(yaml_fname, "r") as file_handle: calib_data = yaml.load(file_handle) camera_info_msg = CameraInfo() camera_info_msg.width = calib_data["image_width"] camera_info_msg.height = calib_data["image_height"] camera_info_msg.K = calib_data["camera_matrix"]["data"] camera_info_msg.D = calib_data["distortion_coefficients"]["data"] camera_info_msg.R = calib_data["rectification_matrix"]["data"] camera_info_msg.P = calib_data["projection_matrix"]["data"] camera_info_msg.distortion_model = calib_data["distortion_model"] return camera_info_msg
def get_cam_info(self, cam_name): cam_name = "calibrations/" + cam_name + ".yaml" stream = file(os.path.join(os.path.dirname(__file__), cam_name), 'r') calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] return cam_info
def makeROSInfo(image): ci = CameraInfo() head = Header() head.stamp = rospy.Time.now() ci.header = head w,h = image.shape[:2] ci.width = w ci.height = h ci.distortion_model = 'plumb_bob' return ci
def publish(self, event): if self.imgmsg is None: return now = rospy.Time.now() # setup ros message and publish with self.lock: self.imgmsg.header.stamp = now self.imgmsg.header.frame_id = self.frame_id self.pub.publish(self.imgmsg) if self.publish_info: info = CameraInfo() info.header.stamp = now info.header.frame_id = self.frame_id info.width = self.imgmsg.width info.height = self.imgmsg.height self.pub_info.publish(info)
def GetCameraInfo(width, height): cam_info = CameraInfo() cam_info.width = width cam_info.height = height cam_info.distortion_model = model #cam_info.D = [0.0]*5 #cam_info.K = [0.0]*9 #cam_info.R = [0.0]*9 #cam_info.P = [0.0]*12 cam_info.D = D cam_info.K = K cam_info.R = R cam_info.P = P cam_info.binning_x = 0 cam_info.binning_y = 0 return cam_info
def pickle_to_info(info_pickle): info = CameraInfo() info.header.stamp = rospy.Time() info.header.seq = info_pickle.header.seq info.header.frame_id = info_pickle.header.frame_id info.roi.x_offset = info_pickle.roi.x_offset info.roi.y_offset = info_pickle.roi.y_offset info.roi.height = info_pickle.roi.height info.roi.width = info_pickle.roi.width info.height = info_pickle.height info.width = info_pickle.width info.D = info_pickle.D info.K = info_pickle.K info.R = info_pickle.R info.P = info_pickle.P return info
def publish(self): now = rospy.Time.now() bridge = cv_bridge.CvBridge() img_bgr = cv2.imread(self.file_name) if img_bgr is None: jsk_logwarn('cannot read the image at {}' .format(self.file_name)) return # resolve encoding encoding = rospy.get_param('~encoding', 'bgr8') if getCvType(encoding) == 0: # mono8 img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) elif getCvType(encoding) == 2: # 16UC1 img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) img = img.astype(np.float32) img = img / 255 * (2 ** 16) img = img.astype(np.uint16) elif getCvType(encoding) == 5: # 32FC1 img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) img = img.astype(np.float32) img /= 255 elif getCvType(encoding) == 16: # 8UC3 if encoding in ('rgb8', 'rgb16'): # RGB img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB) else: img = img_bgr else: jsk_logerr('unsupported encoding: {0}'.format(encoding)) return # setup ros message and publish imgmsg = bridge.cv2_to_imgmsg(img, encoding=encoding) imgmsg.header.stamp = now imgmsg.header.frame_id = self.frame_id self.pub.publish(imgmsg) if self.publish_info: info = CameraInfo() info.header.stamp = now info.header.frame_id = self.frame_id info.width = imgmsg.width info.height = imgmsg.height self.pub_info.publish(info)
def default(self, ci='unused'): if not self.component_instance.capturing: return # press [Space] key to enable capturing image_local = self.data['image'] image = Image() image.header = self.get_ros_header() image.height = self.component_instance.image_height image.width = self.component_instance.image_width image.encoding = self.encoding image.step = image.width * 4 # VideoTexture.ImageRender implements the buffer interface image.data = bytes(image_local) # fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = self.data['intrinsic_matrix'] camera_info = CameraInfo() camera_info.header = image.header camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = 'plumb_bob' camera_info.D = [0] camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]] camera_info.R = R camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx, intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty, intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0] if self.pub_tf: self.publish_with_robot_transform(image) else: self.publish(image) self.topic_camera_info.publish(camera_info)
def parse_yaml(filename): stream = file(filename, "r") calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data["image_width"] cam_info.height = calib_data["image_height"] cam_info.K = calib_data["camera_matrix"]["data"] cam_info.D = calib_data["distortion_coefficients"]["data"] cam_info.R = calib_data["rectification_matrix"]["data"] cam_info.P = calib_data["projection_matrix"]["data"] cam_info.distortion_model = calib_data["distortion_model"] cam_info.binning_x = calib_data["binning_x"] cam_info.binning_y = calib_data["binning_y"] cam_info.roi.x_offset = calib_data["roi"]["x_offset"] cam_info.roi.y_offset = calib_data["roi"]["y_offset"] cam_info.roi.height = calib_data["roi"]["height"] cam_info.roi.width = calib_data["roi"]["width"] cam_info.roi.do_rectify = calib_data["roi"]["do_rectify"] return cam_info
def fetch_image(self, cam): cam.simulate() if not cam.pixels: return None, None cv_img = cv.CreateImageHeader((cam.width , cam.height), cv.IPL_DEPTH_8U, 3) cv.SetData(cv_img, cam.pixels, cam.width*3) cv.ConvertImage(cv_img, cv_img, cv.CV_CVTIMG_FLIP) im = self.bridge.cv_to_imgmsg(cv_img, "rgb8") caminfo = CameraInfo() caminfo.header = im.header caminfo.height = cam.height caminfo.width = cam.width caminfo.D = 5*[0.] caminfo.K = sum([list(r) for r in cam.K],[]) caminfo.P = sum([list(r) for r in cam.P],[]) caminfo.R = sum([list(r) for r in cam.R],[]) return im, caminfo
def parse_yaml(filename): stream = file(filename, 'r') calib_data = yaml.load(stream) cam_info = CameraInfo() cam_info.width = calib_data['image_width'] cam_info.height = calib_data['image_height'] cam_info.K = calib_data['camera_matrix']['data'] cam_info.D = calib_data['distortion_coefficients']['data'] cam_info.R = calib_data['rectification_matrix']['data'] cam_info.P = calib_data['projection_matrix']['data'] cam_info.distortion_model = calib_data['distortion_model'] cam_info.binning_x = calib_data['binning_x'] cam_info.binning_y = calib_data['binning_y'] cam_info.roi.x_offset = calib_data['roi']['x_offset'] cam_info.roi.y_offset = calib_data['roi']['y_offset'] cam_info.roi.height = calib_data['roi']['height'] cam_info.roi.width = calib_data['roi']['width'] cam_info.roi.do_rectify = calib_data['roi']['do_rectify'] return cam_info
def build_camera_info(self): """ computing camera info camera info doesn't change over time """ camera_info = CameraInfo() camera_info.header.frame_id = self.name camera_info.width = self.carla_object.ImageSizeX camera_info.height = self.carla_object.ImageSizeY camera_info.distortion_model = 'plumb_bob' cx = self.carla_object.ImageSizeX / 2.0 cy = self.carla_object.ImageSizeY / 2.0 fx = self.carla_object.ImageSizeX / ( 2.0 * math.tan(self.carla_object.FOV * math.pi / 360.0)) fy = fx camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] camera_info.D = [0, 0, 0, 0, 0] camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0] camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0] self._camera_info = camera_info
def test(): rospy.wait_for_service(service_name) # build theoretical calibration for Unibrain Fire-i camera in # 640x480 mode cinfo = CameraInfo() cinfo.width = 1360 cinfo.height = 1024 #cx = (cinfo.width - 1.0)/2.0 #cy = (cinfo.height - 1.0)/2.0 #fx = fy = 0.0043 # Unibrain Fire-i focal length #fx = 1046.16197 #fy = 1043.87094 #cx = 695.62128 #cy = 544.32183 #k1 = -0.23268 #k2 = 0.08580 #p1 = 0.00098 #p2 = -0.00022 #cinfo.K = [fx, 0., cx, 0., fy, cy, 0., 0., 1.] #cinfo.R = [1., 0., 0., 0., 1., 0., 0., 0., 1.] #cinfo.P = [fx, 0., cx, 0., 0., fy, cy, 0., 0., 0., 1., 0.] cinfo.K = [430.21554970319971, 0.0, 306.6913434743704, 0.0, 430.53169252696676, 227.22480030078816, 0.0, 0.0, 1.0] cinfo.D = [-0.33758562758914146, 0.11161239414304096, -0.00021819272592442094, -3.029195446330518e-05, 0] #cinfo.K = [4827.94, 0, 1223.5, 0, 4835.62, 1024.5, 0, 0, 1] #cinfo.D = [-0.41527, 0.31874, -0.00197, 0.00071, 0] #cinfo.R = [1, 0, 0, 0, 1, 0, 0, 0, 1] #cinfo.P = [4827.94, 0, 1223.5, 0, 0, 4835.62, 1024.5, 0, 0, 0, 1, 0] try: set_camera_info = rospy.ServiceProxy(service_name, SetCameraInfo) response = set_camera_info(cinfo) rospy.loginfo("set_camera_info: success=" + str(response.success) + ", status=" + str(response.status_message)) return response.success except rospy.ServiceException, e: print "Service call failed: %s"%e
def get_camera_info(hard_coded=True): if hard_coded: cx = 319.5 cy = 239.5 fx = 525.5 fy = 525.5 return (cx, cy, fx, fy) #if we are using a different camera, then #we can listen to the ros camera info topic for that device #and get our values here. else: import image_geometry from sensor_msgs.msg import CameraInfo cam_info = CameraInfo() cam_info.height = 480 cam_info.width = 640 cam_info.distortion_model = "plumb_bob" cam_info.D = [0.0, 0.0, 0.0, 0.0, 0.0] cam_info.K = [525.0, 0.0, 319.5, 0.0, 525.0, 239.5, 0.0, 0.0, 1.0] cam_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] cam_info.P = [525.0, 0.0, 319.5, 0.0, 0.0, 525.0, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0] cam_info.binning_x = 0 cam_info.binning_y = 0 cam_info.roi.x_offset = 0 cam_info.roi.y_offset = 0 cam_info.roi.height = 0 cam_info.roi.width = 0 cam_info.roi.do_rectify = False camera_model = image_geometry.PinholeCameraModel() camera_model.fromCameraInfo(cam_info) return camera_model.cx(), camera_model.cy(), camera_model.fx(), camera_model.fy()
def dr_callback(self, config, level): ci = CameraInfo() ci.header.stamp = rospy.Time.now() ci.width = config['width'] ci.height = config['height'] ci.distortion_model = config['distortion_model'] ci.D = [config['d0'], config['d1'], config['d2'], config['d3'], config['d4']] ci.K[0 * 3 + 0] = config['fx'] ci.K[0 * 3 + 2] = config['cx'] ci.K[1 * 3 + 1] = config['fy'] ci.K[1 * 3 + 2] = config['cy'] ci.K[2 * 3 + 2] = 1 ci.P[0 * 4 + 0] = config['fx'] ci.P[0 * 4 + 2] = config['cx'] ci.P[1 * 4 + 1] = config['fy'] ci.P[1 * 4 + 2] = config['cy'] ci.P[2 * 4 + 2] = 1 ci.R[0] = 1 ci.R[4] = 1 ci.R[8] = 1 self.camera_info = ci self.pub.publish(ci) return config
def loadCalibrationFile(filename, cname): """ Load calibration data from a file. This function returns a `sensor_msgs/CameraInfo`_ message, based on the filename parameter. An empty or non-existent file is *not* considered an error; a null CameraInfo being provided in that case. :param filename: location of CameraInfo to read :param cname: Camera name. :returns: `sensor_msgs/CameraInfo`_ message containing calibration, if file readable; null calibration message otherwise. :raises: :exc:`IOError` if an existing calibration file is unreadable. """ ci = CameraInfo() try: f = open(filename) calib = yaml.load(f) if calib is not None: if calib['camera_name'] != cname: rospy.logwarn("[" + cname + "] does not match name " + calib['camera_name'] + " in file " + filename) # fill in CameraInfo fields ci.width = calib['image_width'] ci.height = calib['image_height'] ci.distortion_model = calib['distortion_model'] ci.D = calib['distortion_coefficients']['data'] ci.K = calib['camera_matrix']['data'] ci.R = calib['rectification_matrix']['data'] ci.P = calib['projection_matrix']['data'] except IOError: # OK if file did not exist pass return ci
def test_with_known_image(self): filename = rospy.get_param("~filename") self.setup() # Setup the node # Publish the camera info msg_info = CameraInfo() msg_info.height = 480 msg_info.width = 640 msg_info.K = [331.026328, 0.0, 319.035097, 0.0, 335.330339, 216.450133, 0.0, 0.0, 1.0] self.pub_info.publish(msg_info) # Publish the test image img = cv2.imread(filename) cvb = CvBridge() msg_raw = cvb.cv2_to_imgmsg(img, encoding="bgr8") self.pub_raw.publish(msg_raw) # Wait for the message to be received timeout = rospy.Time.now() + rospy.Duration(5) # Wait at most 5 seconds for the node to reply while not self.msg_received and not rospy.is_shutdown() and rospy.Time.now() < timeout: rospy.sleep(0.1) self.assertLess(rospy.Time.now(), timeout, "Waiting for apriltag detection timed out.") # Check that an apriltag with id=108 was detected found = 0 for tag in self.msg_tags.detections: if tag.id == 108: found += 1 self.assertAlmostEqual(tag.pose.pose.position.x, 0.0, delta=0.1) # Allow 10 cm of error margin self.assertAlmostEqual(tag.pose.pose.position.y, 0.0, delta=0.1) # Allow 10 cm of error margin self.assertAlmostEqual(tag.pose.pose.position.z, 0.305, delta=0.1) # Allow 10 cm of error margin # Convert the quat to an angle about z rot = tag.pose.pose.orientation ang = tr.euler_from_quaternion((rot.x, rot.y, rot.z, rot.w))[2] # z axis angle only self.assertAlmostEqual(ang, 0, delta=15 * np.pi / 180) # Allow up to 15 degrees of error margin self.assertGreaterEqual(found, 1, "Expected apriltag with id=108 not found.")
def stream(self): url = 'http://%s/nphMotionJpeg' % self.axis.hostname url = url + "?resolultion=%dx%d" % (self.width, self.height) fp = urllib.urlopen(url) print 'Streaming \n' while self.notdone: boundary = fp.readline() header = {} while 1: line = fp.readline() if line == "\r\n": break line = line.strip() parts = line.split(": ", 1) header[parts[0]] = parts[1] content_length = int(header['Content-length']) img = fp.read(content_length) line = fp.readline() msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = img self.axis.pub.publish(msg) cimsg = CameraInfo() cimsg.header.stamp = msg.header.stamp cimsg.width = self.width cimsg.height = self.height self.axis.caminfo_pub.publish(cimsg)
import pyrealsense2 as rs import control_test as ct import rospy, tf, sys, time from geometry_msgs.msg import Pose, PoseStamped from sensor_msgs.msg import Image from sensor_msgs.msg import CameraInfo import tf2_ros # camera matrix of realsense415 with half resolution camera_info = CameraInfo() camera_info.K = [ 931.6937866210938, 0.0, 624.7894897460938, 0.0, 931.462890625, 360.5186767578125, 0.0, 0.0, 1.0 ] camera_info.header.frame_id = 'camera_color_optical_frame' camera_info.height = 720 camera_info.width = 1280 workspace_limits = np.asarray([ [0.3 - 0.08, 0.3 + 0.08], [-0.524 - 0.1, -0.524 + 0.1], [0.5 - 0.02, 0.5 + 0.02] ]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates) calib_grid_step = 0.04 checkerboard_size = (3, 3) refine_criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) calib_grid_step = 0.04 chessboard_size = (3, 3) refine_criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
def run(self): img = Image() r = rospy.Rate(self.config['frame_rate']) while self.is_looping(): if self.pub_img_.get_num_connections() == 0: if self.nameId: rospy.loginfo( 'Unsubscribing from camera as nobody listens to the topics.' ) self.camProxy.unsubscribe(self.nameId) self.nameId = None r.sleep() continue if self.nameId is None: self.reconfigure(self.config, 0) r.sleep() continue image = self.camProxy.getImageRemote(self.nameId) if image is None: continue # Deal with the image if self.config['use_ros_time']: img.header.stamp = rospy.Time.now() else: img.header.stamp = rospy.Time(image[4] + image[5] * 1e-6) img.header.frame_id = self.frame_id img.height = image[1] img.width = image[0] nbLayers = image[2] if image[3] == kYUVColorSpace: encoding = "mono8" elif image[3] == kRGBColorSpace: encoding = "rgb8" elif image[3] == kBGRColorSpace: encoding = "bgr8" elif image[3] == kYUV422ColorSpace: encoding = "yuv422" # this works only in ROS groovy and later elif image[3] == kDepthColorSpace or image[ 3] == kRawDepthColorSpace: encoding = "16UC1" else: rospy.logerr("Received unknown encoding: {0}".format(image[3])) img.encoding = encoding img.step = img.width * nbLayers img.data = image[6] self.pub_img_.publish(img) # deal with the camera info if self.config['source'] == kDepthCamera and ( image[3] == kDepthColorSpace or image[3] == kRawDepthColorSpace): infomsg = CameraInfo() # yes, this is only for an XTion / Kinect but that's the only thing supported by NAO ratio_x = float(640) / float(img.width) ratio_y = float(480) / float(img.height) infomsg.width = img.width infomsg.height = img.height # [ 525., 0., 3.1950000000000000e+02, 0., 525., 2.3950000000000000e+02, 0., 0., 1. ] infomsg.K = [ 525, 0, 3.1950000000000000e+02, 0, 525, 2.3950000000000000e+02, 0, 0, 1 ] infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0, 0, 525, 2.3950000000000000e+02, 0, 0, 0, 1, 0 ] for i in range(3): infomsg.K[i] = infomsg.K[i] / ratio_x infomsg.K[3 + i] = infomsg.K[3 + i] / ratio_y infomsg.P[i] = infomsg.P[i] / ratio_x infomsg.P[4 + i] = infomsg.P[4 + i] / ratio_y infomsg.D = [] infomsg.binning_x = 0 infomsg.binning_y = 0 infomsg.distortion_model = "" infomsg.header = img.header self.pub_info_.publish(infomsg) elif self.config['camera_info_url'] in self.camera_infos: infomsg = self.camera_infos[self.config['camera_info_url']] infomsg.header = img.header self.pub_info_.publish(infomsg) r.sleep() if (self.nameId): rospy.loginfo("unsubscribing from camera ") self.camProxy.unsubscribe(self.nameId)
image_list_depth = recursive_glob(directory, "*_depth.png", []) image_list_rgb.sort() image_list_depth.sort() rgb_pub = rospy.Publisher(rgb_topic_to_stream, Image, queue_size=10) depth_pub = rospy.Publisher(depth_topic_to_stream, Image, queue_size=10) camera_info_pub = rospy.Publisher(camera_info_topic_to_stream, CameraInfo, queue_size=10) names_pub = rospy.Publisher(names_topic_to_stream, std_msgs.msg.String, queue_size=10) yaml_data = numpy.asarray(cv2.cv.Load(directory + "/calib_ir.yaml")) cam_info = CameraInfo() cam_info.height = 424 cam_info.width = 512 cam_info.distortion_model = 'plumb_bob' cam_info.K[0] = yaml_data[0, 0] cam_info.K[2] = yaml_data[0, 2] cam_info.K[4] = yaml_data[1, 1] cam_info.K[5] = yaml_data[1, 2] cam_info.D = [0.0, 0.0, 0.0, 0.0, 0.0] cam_info.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] cam_info.P[0 * 4 + 0] = yaml_data[0, 0] cam_info.P[0 * 4 + 2] = yaml_data[0, 2] cam_info.P[1 * 4 + 1] = yaml_data[1, 1] cam_info.P[1 * 4 + 2] = yaml_data[1, 2] cam_info.P[2 * 4 + 2] = 1 for i in xrange(len(image_list_rgb)): print(str(i).zfill(4) + " / " + str(len(image_list_rgb)))
def run(self): # left_cam_info = self.yaml_to_camera_info(self.left_yaml_file) # right_cam_info = self.yaml_to_camera_info(self.right_yaml_file) left_cam_info = CameraInfo() left_cam_info.width = 640 left_cam_info.height = 480 left_cam_info.K = [ 883.998642, 0.000000, 349.540253, 0.000000, 887.969815, 247.902874, 0.000000, 0.000000, 1.000000 ] left_cam_info.D = [0.125962, -0.905045, 0.006512, 0.007531, 0.000000] left_cam_info.R = [ 0.985389, 0.006639, 0.170189, -0.004920, 0.999933, -0.010521, -0.170248, 0.009530, 0.985355 ] left_cam_info.P = [ 1022.167889, 0.000000, 150.220785, 0.000000, 0.000000, 1022.167889, 249.024044, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000 ] left_cam_info.distortion_model = 'plumb_bob' right_cam_info = CameraInfo() right_cam_info.width = 640 right_cam_info.height = 480 right_cam_info.K = [ 874.019843, 0.000000, 331.121922, 0.000000, 875.918758, 244.867443, 0.000000, 0.000000, 1.000000 ] right_cam_info.D = [0.041385, -0.059698, 0.005392, 0.009075, 0.000000] right_cam_info.R = [ 0.976610, 0.003803, 0.214985, -0.005979, 0.999937, 0.009472, -0.214936, -0.010535, 0.976571 ] right_cam_info.P = [ 1022.167889, 0.000000, 150.220785, -41.006903, 0.000000, 1022.167889, 249.024044, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000 ] right_cam_info.distortion_model = 'plumb_bob' rate = rospy.Rate(20) while not rospy.is_shutdown(): ret, frame = self.cam.read() if not ret: print('[ERROR]: frame error') break expand_frame = cv2.resize(frame, None, fx=2, fy=1) left_image = expand_frame[0:480, 0:640] right_image = expand_frame[0:480, 640:1280] self.msg_header.frame_id = 'stereo_image' self.msg_header.stamp = rospy.Time.now() left_cam_info.header = self.msg_header right_cam_info.header = self.msg_header self.left_image_info_pub.publish(left_cam_info) self.right_image_info_pub.publish(right_cam_info) # self.pub_image(self.left_image_pub, left_image, self.msg_header ) # self.pub_image(self.right_image_pub, right_image, self.msg_header ) try: thread.start_new_thread(self.pub_image, ( self.left_image_pub, left_image, self.msg_header, )) thread.start_new_thread(self.pub_image, ( self.right_image_pub, right_image, self.msg_header, )) except: print("[ERROR]: unable to start thread") rate.sleep()
def camcallback(self, data): now = rospy.Time.now() #send transforms to show each camera's coordinate system br = tf.TransformBroadcaster() br.sendTransform(self.pos1, self.quat1, now, '/cam1', 'world') br.sendTransform(self.pos2, self.quat2, now, '/cam2', 'world') #look up the transform to the ball try: (ballpos, ballrot) = self.listener.lookupTransform('/ball', 'world', rospy.Time(0)) (ballpos1, ballrot1) = self.listener.lookupTransform('/ball', 'cam1', rospy.Time(0)) (ballpos2, ballrot2) = self.listener.lookupTransform('/ball', 'cam2', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print("exception!") ballpos, ballrot = [0, 0, 0], [0, 0, 0, 1] ballpos1, ballrot1 = [0, 0, 0], [0, 0, 0, 1] ballpos2, ballrot2 = [0, 0, 0], [0, 0, 0, 1] #continue #generate the synthetic images im1 = self.simcam1.renderSphere(ballpos, 0.05, ballpos1[2]) im2 = self.simcam2.renderSphere(ballpos, 0.05, ballpos2[2]) #now publish the synthetic images im1out = self.bridge.cv2_to_imgmsg(im1, "bgr8") im1out.header.stamp = now im2out = self.bridge.cv2_to_imgmsg(im2, "bgr8") im2out.header.stamp = now #now prep the camera info msgs CI1 = CameraInfo() CI2 = CameraInfo() CI1.header.stamp = rospy.Time.now() CI1.header.frame_id = '/cam1' CI1.height = self.simcam1.h CI1.width = self.simcam1.w CI1.K = self.simcam1.K[0:3, 0:3].reshape(1, 9)[0] CI1.R = self.simcam1.R[0:3, 0:3].reshape(1, 9)[0] CI1.P = self.simcam1.P[0:3, :].reshape(1, 12)[0] CI2.header.stamp = rospy.Time.now() CI2.header.frame_id = '/cam2' CI2.height = self.simcam2.h CI2.width = self.simcam2.w CI2.K = self.simcam2.K[0:3, 0:3].reshape(1, 9)[0] CI2.R = self.simcam2.R[0:3, 0:3].reshape(1, 9)[0] CI2.P = self.simcam2.P[0:3, :].reshape(1, 12)[0] self.cam1infopub.publish(CI1) self.cam2infopub.publish(CI2) #publish the two synthetic images self.im1_pub.publish(im1out) self.im2_pub.publish(im2out)
default='*', type=str) args = parser.parse_args() filename = args.filename print(filename) # Parse yaml file # Load data from file with open(filename, "r") as file_handle: calib_data = yaml.load(file_handle) # Parse camera_info_msg = CameraInfo() camera_info_msg.width = calib_data["image_width"] camera_info_msg.height = calib_data["image_height"] camera_info_msg.K = calib_data["camera_matrix"]["data"] camera_info_msg.D = calib_data["distortion_coefficients"]["data"] camera_info_msg.R = calib_data["rectification_matrix"]["data"] camera_info_msg.P = calib_data["projection_matrix"]["data"] camera_info_msg.distortion_model = calib_data["distortion_model"] # Initialize publisher node rospy.init_node("camera_info_publisher", anonymous=True) publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=10) rate = rospy.Rate(10) # Run publisher while not rospy.is_shutdown(): publisher.publish(camera_info_msg) rate.sleep()
#!/usr/bin/env python import rospy from sensor_msgs.msg import CameraInfo, PointCloud2 if __name__ == "__main__": rospy.init_node("sample_camera_info_and_pointcloud_publisher") pub_info = rospy.Publisher("~info", CameraInfo, queue_size=1) pub_cloud = rospy.Publisher("~cloud", PointCloud2, queue_size=1) rate = rospy.Rate(1) while not rospy.is_shutdown(): info = CameraInfo() info.header.stamp = rospy.Time.now() info.header.frame_id = "origin" info.height = 544 info.width = 1024 info.D = [ -0.20831339061260223, 0.11341656744480133, -0.00035378438769839704, -1.746419547998812e-05, 0.013720948249101639, 0.0, 0.0, 0.0 ] info.K = [ 598.6097412109375, 0.0, 515.5960693359375, 0.0, 600.0813598632812, 255.42999267578125, 0.0, 0.0, 1.0 ] info.R = [ 0.999993085861206, 0.0022128731943666935, -0.0029819998890161514, -0.0022144035901874304, 0.9999974370002747, -0.0005100672133266926, 0.002980863442644477, 0.0005166670307517052, 0.9999954104423523 ] info.P = [ 575.3445434570312, 0.0, 519.5, 0.0, 0.0, 575.3445434570312, 259.5,
def timer_cb(event): # info_msg.header.stamp = rospy.Time.now() info_msg.header.stamp = rospy.Time(0) pub_info.publish(info_msg) if __name__ == '__main__': rospy.init_node('static_virtual_camera') pub_info = rospy.Publisher('~camera_info', CameraInfo, queue_size=1) info_msg = CameraInfo() info_msg.header.frame_id = 'static_virtual_camera' # info_msg.height = 640 info_msg.height = 240 info_msg.width = 240 # fovx = 98 fovx = 11.421 # fovy = 114 # fovy = 98 fovy = 11.421 fx = info_msg.width / 2.0 / \ np.tan(np.deg2rad(fovx / 2.0)) fy = info_msg.height / 2.0 / \ np.tan(np.deg2rad(fovy / 2.0)) cx = info_msg.width / 2.0 cy = info_msg.height / 2.0 info_msg.D = [-0.071069, 0.024841, -6.7e-05, 0.024236, 0.0] info_msg.K = np.array([fx, 0, cx, 0, fy, cy, 0, 0, 1.0])
def camera_info_back(self): msg_header = Header() msg_header.frame_id = "f450/robot_camera_down" msg_roi = RegionOfInterest() msg_roi.x_offset= 0 msg_roi.y_offset= 0 msg_roi.height = 0 msg_roi.width = 0 msg_roi.do_rectify=0 msg= CameraInfo() msg.header = msg_header msg.height = 480 msg.width = 640 msg.distortion_model = 'plumb_bob' msg.D = [0.0, 0.0, 0.0, 0.0, 0.0] msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0,1.0] msg.R = [1.0, 0.0,0.0,0.0,0.866,0.5,0.0,-0.5,8.66] msg.P = [1.0, 0.0, 320.5,0.0,0.0,1.0,240.5,0.0,0.0,0.0,1.0,0.0] #same as the front camera msg.binning_x = 0 msg.binning_y = 0 msg.roi = msg_roi return msg def camera_info_front(self): msg_header = Header() msg_header.frame_id = "f450/robot_camera" msg_roi = RegionOfInterest() msg_roi.x_offset= 0 msg_roi.y_offset= 0 msg_roi.height = 0 msg_roi.width = 0 msg_roi.do_rectify=0 msg= CameraInfo() msg.header = msg_header msg.height = 480 msg.width = 640 msg.distortion_model = 'plumb_bob' msg.D = [0.0, 0.0, 0.0, 0.0, 0.0] msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0,1.0] msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,0.0, 1.0] msg.P = [1.0, 0.0, 320.5,0.0,0.0,1.0,240.5,0.0,0.0,0.0,1.0,0.0] msg.binning_x = 0 msg.binning_y = 0 msg.roi = msg_roi return msg def convert_pixel_camera(self): pass def mocap_cb(self,msg): self.curr_pose = msg def state_cb(self,msg): if msg.mode == 'OFFBOARD': self.isReadyToFly = True else: print msg.mode def update_state_cb(self,data): self.mode= data.data print self.mode def tag_detections(self,msgs): rate = rospy.Rate(30) if len(msgs.detections)>0: msg = msgs.detections[0].pose # The first element in the array is the pose self.tag_pt= msg.pose.pose.position self.pub.publish("FOUND UAV") else: if self.mode == "DESCENT": self.pub.publish("MISSING UAV") def get_descent(self, x, y,z): des_vel = PositionTarget() des_vel.header.frame_id = "world" des_vel.header.stamp= rospy.Time,from_sec(time.time()) des_vel.coordinate_frame= 8 des_vel.type_mask = 3527 des_vel.velocity.x = x des_vel.velocity.y = y des_vel.velocity.z = z return des_vel def lawnmover(self, rect_x, rect_y, height, offset, offset_x): if len(self.loc)== 0 : takeoff = [self.curr_pose.pose.postion.x, self.curr_pose.pose.postion.y, height , 0 , 0 , 0 , 0] move_to_offset = [self.curr_pose.pose.postion_x + offset, self.curr_posr.pose.postion_y - rect_y/2, height, 0 , 0 , 0 ,0 ] self.loc.append(takeoff) self.loc.append(move_to_offset) left = True while True: if left: x = self.loc[len(self.loc)-1][0] y = self.loc[len(self.loc)-1][1] + rect_y/3 z = self.loc[len(self.loc)-1][2] self.loc.append([x,y,z,0,0,0,0]) x = self.loc[len(self.loc)-1][0] y = self.loc[len(self.loc)-1][1] + rect_y/3 z = self.loc[len(self.loc)-1][2] self.loc.append([x,y,z,0,0,0,0]) x = self.loc[len(self.loc)-1][0] y = self.loc[len(self.loc)-1][1] + rect_y/3 z = self.loc[len(self.loc)-1][2] left = False x = self.loc[len(self.loc)-1][0] + offset_x y = self.loc[len(self.loc)-1][0] z = self.loc[len(self.loc)-1][0] self.loc.append([x,y,z,0,0,0,0]) if x > rect_x : break else: x = self.loc[len(self.loc)-1][0] y = self.loc[len(self.loc)-1][1]-rect_y/3 z = self.loc[len(self.loc)-1][2] self.loc.append([x,y,z,0,0,0,0]) x = self.loc[len(self.loc)-1][0] y = self.loc[len(self.loc)-1][1]-rect_y/3 z = self.loc[len(self.loc)-1][2] self.loc.append([x,y,z,0,0,0,0]) x = self.loc[len(self.loc)-1][0] y = self.loc[len(self.loc)-1][1]-rect_y/3 z = self.loc[len(self.loc)-1][2] self.loc.append([x,y,z,0,0,0,0]) left = True x = self.loc[len(self.loc)-1][0]+ offset_x y = self.loc[len(self.loc)-1][1] z = self.loc[len(self.loc)-1][2] self.loc.append([x,y,z,0,0,0,0]) if x > rect_x: break rate = rospy.Rate(10) self.des_pose = self.copy_pose(self.curr_pose) #Why do we need to copy curr_pose into des_pose shape = len(self.loc) pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size = 10) print self.mode while self.mode == "SURVEY" and not rospy.is_shutdown(): if self.waypointIndex == shape : self.waypointIndex = 1 # resetting the waypoint index if self.isReadyToFly: des_x = self.loc[self.waypointIndex][0] des_y = self.loc[self.waypointIndex][1] des_z = self.loc[self.waypointIndex][2] self.des_pose.pose.position.x = des_x self.des_pose.pose.position.y = des_y self.des_pose.pose.position.z = des_z self.des_pose.pose.orientation.x = self.loc[self.waypointIndex][3] self.des_pose.pose.orientation.y = self.loc[self.waypointIndex][4] self.des_pose.pose.orientation.z = self.loc[self.waypointIndex][5] self.des_pose.pose.orientation.w = self.loc[self.waypointIndex][6] curr_x = self.curr_pose.pose.position.x curr_y = self.curr_pose.pose.position.y curr_z = self.curr_pose.pose.position.z dist = math.sqrt((curr_x - des_x)*(curr_x - des_x) + (curr_y - des_y)*(curr_y - des_y ) + (curr_z - des_z)(curr_z - des_z)) if dist < self.distThreshold: self.waypointIndex += 1 pose_pub.publish(self.des_pose) rate.sleep() def hover(self): location = self.hover_loc loc = [location, location, location, location, location, location, location, location, loaction] rate = rospy.Rate(10) rate.sleep() sharp = len(loc) pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size= 10) des_pose = self.copy_pose(self.curr_pose) waypoint_index = 0 sim_ctr = 1 print self.mode while self.mode == "HOVER" and not rospy.is_shutdown(): if waypoint_index==shape: waypoint_index = 0 # changing the way point index to 0 sim_ctr = sim_ctr + 1 print "HOVER STOP COUNTER:" + str(sim_ctr) if self.isReadyToFly: des_x = loc[waypoint_index][0] des_y = loc[waypoint_index][1] des_z = loc[waypoint_index][2] des_pose.pose.position.x = des_x des_pose.pose.position.y = des_y des_pose.pose.position.z = des_z des_pose.pose.orientation.x = loc[waypoint_index][3] des_pose.pose.orientation.y = loc[waypoint_index][4] des_pose.pose.orientation.z = loc[waypoint_index][5] des_pose.pose.oirentation.w = loc[waypoint_index][6] curr_x = self.curr_pose.pose.position.x curr_y = self.curr_pose.pose.position.y curr_z = self.curr_pose.pose.position.z dist = math.sqrt((curr_x - des_x)*(curr_x - des_x) + (curr_y - des_y)(curr_y - des_y) + (curr_z - des_z)*(curr_z - des_z)) if dist<self.distThreshold : waypoint_index += 1 if dist < self.distThreshold: waypoint_index += 1 if sim_ctr == 50: pass pose_pub.publish(des_pose) rate.sleep() def scan(self, rect_y, offset_x): move= "" rate=rospy.Rate(10) if self.waypointIndex %4 ==1: move="back" elif((self.waypointIndex + (self.waypointIndex % 4))/4)%2 == 0: move = "right" else: move = "left" print self.mode loc = self.curr_pose.pose.position print loc print rect_y print offset_x while self.mode == "SCAN" and not rospy.is_shutdown(): if move == "left": self.vel_pub.publish(self.get_decent(0,0.5,0.1)) if abs(self.curr_pose.pose.position.y - loc.y) > rect_y/3 self.pub.publish("SCAN COMPLETE") elif move == "right": self.vel_pub.publish(self.get_decent(0,-0.5,0.1)) if abs(self.curr_pose.pose.postion.y - loc.y) > rect_y/3 self.pub.publish("SCAN COMPLETE") elif move == "back": self.vel_pub.publish(self.get_decent(-0.3,0,1)) if abs(self.curr_pose.pose.position.x - loc.x) > offset_x: self.pub.publish("SCAN COMPLETE") else: print "move error" print abs(self.curr_pose.pose.position.y - loc.y) print abs(self.curr_pose.pose.position.x - loc.x) rate.sleep() def descent(self): rate = rospy.Rate(10) # 10 Hz time_step = 1/10 print self.mode self.x_change = 1 self.y_change = 1 self.x_prev_error = 0 self.y_prev_error = 0 self.x_sum_error=0 self.y_sum_error=0 self.x_exp_movement=0 self.y_exp_movement=0 self.x_track = 0 self.y_track = 0 self.curr_xerror=0 self.curr_yerror=0 while self.mode == "DESCENT" and self.curr_pose.pose.position.z > 0.2 and not rospy.is_shutdown(): err_x = self.curr_pose.pose.position.x - self.tag_pt.x err_y = self.curr_pose.pose.position.y - self.tag_pt.y self.x_change += err_x*self.KP + (((self.x_prev_error-self.curr_xerror)/time_step)* self.KD) + (self.x_sum_error * self.KI*time_step) self.y_change += err_y*self.KP + (((self.y_prev_error-self.curr_yerror)/time_step)* self.KD) + (self.y_sum_error * self.KI*time_step) self.x_change = max(min(0.4,self.x_change), -0.4) self.y_change = max(min(0.4,self.y_change), -0.4) if err_x > 0 and err_y < 0: des = self.get_descent(-1*abs(self.x_change), 1*abs(self.y_change), -0.08) elif err_x > 0 and err_y > 0: des = self.get_descent(-1*abs(self.x_change), -1*abs(self.y_change), -0.08) elif err_x < 0 and err_y > 0: des = self.get_descent(1*abs(self.x_change), -1*abs(self.y_change), -0.08) elif err_x < 0 and err_y < 0: des = self.get_descent(1*abs(self.x_change), 1*abs(self.y_change), -0.08) else: des = self.get_descent(self.x_change, self.y_change, -0.08) self.vel_pub.publish(des) rate.sleep() self.x_exp_movement = self.x_change self.y_exp_movement = self.y_change self.x_track = self.x_exp_movement + self.curr_pose.pose.position.x self.y_track = self.y_exp_movement + self.curr_pose.pose.position.y self.curr_xerror= self.x_track - self.tag_pt.x self.curr_yerror= self.y_track - self.tag_pt.y self.x_prev_error= err_x self.y_prev_error= err_y self.x_sum_error += err_x self.y_sum_error += err_y if self.curr_pose.pose.position.z < 0.2: #Perform the necessary action to complete pickup instruct actuators self.pub.publish("PICKUP COMPLETE") def yolo_descent(self): rate= rospy.Rate(10) print self.mode self.x_change = 1 self.y_change = 1 self.x_prev_error=0 self.y_prev_error=0 self.x_sum_error=0 self.y_sum_error=0 timeout = 120 yolo_KP = 0.08 yolo_KD = 0.004 yolo_KI = 0.0005 while self.mode == "DESCENT" and not rospy.is_shutdown(): err_x = 0 - self.target[0] err_y = 0 - self.target[1] self.x_change += err_x * yolo_KP + (self.x_prev_error * yolo_KD) + (self.x_sum_error * yolo_KI) self.y_change += err_y * yolo_KP + (self.y_prev_error * yolo_KD) + (self.y_sum_error * yolo_KI) self.x_change = max(min(0.4, self.x_change), -0.4) self.y_change = max(min(0.4, self.y_change), -0.4) if err_x > 0 and err_y < 0: des = self.get_descent(-1 * self.x_change, -1 * self.y_change, -0.08) elif err_x < 0 and err_y > 0: des = self.get_descent(-1 * self.x_change, -1 * self.y_change, -0.08) else: des = self.get_descent(self.x_change, self.y_change, -0.08) self.vel_pub.publish(des) timeout -= 1 rate.sleep() self.x_prev_error = err_x self.y_prev_error = err_y self.x_sum_error += err_x self.y_sum_error += err_y if timeout == 0 and self.curr_pose.pose.position.z > 0.7: timeout = 120 print timeout self.x_change = 0 self.y_change = 0 self.x_sum_error = 0 self.y_sum_error = 0 self.x_prev_error = 0 self.y_prev_error = 0 if self.curr_pose.pose.position.z < 0.2: # TODO # self.mode = "HOVER" # PICK UP # self.hover_loc = [self.curr_pose.pose.position.x] # TODO self.pub.publish("PICKUP COMPLETE") def rt_survey(self): location = [self.saved_location.pose.position.x, self.saved_location.pose.position.y, self.saved_location.pose.position.z, 0, 0, 0, 0] loc = [location, location, location, location, location] rate = rospy.Rate(10) rate.sleep() shape = len(loc) pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) des_pose = self.copy_pose(self.curr_pose) waypoint_index = 0 sim_ctr = 1 print self.mode while self.mode == "RT_SURVEY" and not rospy.is_shutdown(): if waypoint_index == shape: waypoint_index = 0 sim_ctr += 1 if self.isReadyToFly: des_x = loc[waypoint_index][0] des_y = loc[waypoint_index][1] des_z = loc[waypoint_index][2] des_pose.pose.position.x = des_x des_pose.pose.position.y = des_y des_pose.pose.position.z = des_z des_pose.pose.orientation.x = loc[waypoint_index][3] des_pose.pose.orientation.y = loc[waypoint_index][4] des_pose.pose.orientation.z = loc[waypoint_index][5] des_pose.pose.orientation.w = loc[waypoint_index][6] curr_x = self.curr_pose.pose.position.x curr_y = self.curr_pose.pose.position.y curr_z = self.curr_pose.pose.position.z dist = math.sqrt((curr_x - des_x)*(curr_x - des_x) + (curr_y - des_y)*(curr_y - des_y) + (curr_z - des_z)*(curr_z - des_z)) if dist < self.distThreshold: waypoint_index += 1 if sim_ctr == 5: self.pub.publish("RTS COMPLETE") self.saved_location = None pose_pub.publish(des_pose) rate.sleep() def controller(self): while not rospy.is_shutdown(): if self.mode == "SURVEY": self.lawnmover(200, 20, 7, 10, 3) if self.mode == "HOVER": self.hover() if self.mode == "SCAN": self.scan(20, 3) # pass x_offset, length of rectangle, to bound during small search if self.mode == "TEST": print self.mode self.vel_pub.publish(self.get_descent(0, 0.1, 0)) if self.mode == "DESCENT": self.descent() if self.mode == "RT_SURVEY": self.rt_survey() def planner(self, msg): if msg.data == "FOUND UAV" and self.mode == "SURVEY": self.saved_location = self.curr_pose self.mode = "SCAN" if msg.data == "FOUND UAV" and self.mode == "SCAN": self.detection_count += 1 print self.detection_count if self.detection_count > 25: self.hover_loc = [self.curr_pose.pose.position.x, self.curr_pose.pose.position.y, self.curr_pose.pose.position.z, 0, 0, 0, 0] self.mode = "HOVER" self.detection_count = 0 if msg.data == "FOUND UAV" and self.mode == "HOVER": print self.detection_count self.detection_count += 1 if self.detection_count > 40: self.mode = "DESCENT" self.detection_count = 0 if msg.data == "MISSING UAV" and self.mode == "DESCENT": self.missing_count += 1 if self.missing_count > 80: self.mode = "HOVER" self.missing_count = 0 if msg.data == "FOUND UAV" and self.mode == "DESCENT": self.missing_count = 0 if msg.data == "SCAN COMPLETE": self.mode = "RT_SURVEY" self.detection_count = 0 if msg.data == "HOVER COMPLETE": if self.waypointIndex == 0: # TODO remove this, this keeps the drone in a loop of search self.mode = "SURVEY" else: self.mode = "RT_SURVEY" self.detection_count = 0 if msg.data == "RTS COMPLETE": self.mode = "SURVEY" if msg.data == "PICKUP COMPLETE": # self.mode = "CONFIRM_PICKUP" # go back and hover at takeoff location self.hover_loc = [self.loc[0][0], self.loc[0][1], self.loc[0][2], 0, 0, 0, 0] self.mode = "HOVER" self.loc = [] self.waypointIndex = 0
rightCamInfo.K = [ 334.3432506979186, 0.0, 258.96387838003477, 0.0, 329.156146359108, 257.21497195869205, 0.0, 0.0, 1.0 ] rightCamInfo.R = [ 0.999662592692041, 0.01954381929425834, -0.017109643468520532, -0.019640898676970494, 0.9997918363216206, -0.0055244116250247315, 0.016998113759693796, 0.005858596421934612, 0.9998383574241275 ] rightCamInfo.P = [ 372.1279396837233, 0.0, 276.3964099884033, 135.43805496802958, 0.0, 372.1279396837233, 262.72875595092773, 0.0, 0.0, 0.0, 1.0, 0.0 ] rightCamInfo.distortion_model = "plumb_bob" rightCamInfo.width = 300 rightCamInfo.height = 300 rightCamInfo.binning_x = 0 rightCamInfo.binning_y = 0 sensor_id = 0, sensor_mode = 3, capture_width = 1280, capture_height = 720, display_width = 1280, display_height = 720, framerate = 30, flip_method = 0, """3264 x 2464 FR = 21.000000 fps Duration = 47619048 ; Analog Gain range min 1.000000, max 10.625000; Exposure Range min 13000, max 683709000;""" gst_pipeline = ( "nvarguscamerasrc sensor-id=0 sensor-mode=0 ! "
import moveit_msgs.srv import geometry_msgs.msg #from io_interface import * from cv_bridge import CvBridge, CvBridgeError from PIL import Image as IM import cv2 import pyrealsense2 as rs # camera matrix of realsense camera_info = CameraInfo() camera_info.K = [ 616.3787841796875, 0.0, 434.0303955078125, 0.0, 616.4257202148438, 234.33065795898438, 0.0, 0.0, 1.0 ] camera_info.header.frame_id = 'camera_color_optical_frame' camera_info.height = 480 camera_info.width = 848 def image_callback(color, depth, color_intrinsics): # TODO: transform sensor_msgs/Image to numpy array and save it jpg files np_arr = np.fromstring(color.data, np.uint8) color_image = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) #im = IM.fromarray(color_image) #im.save("/home/bionicdl/catkin_ws/color_image.jpg") # TODO: detect circles and other features in the color image gray_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) circles = cv2.HoughCircles(gray_image, cv2.HOUGH_GRADIENT,
def post_image(self, component_instance): """ Publish the data of the Camera as a ROS-Image message. """ image_local = component_instance.local_data['image'] if not image_local or image_local == '' or not image_local.image or not component_instance.capturing: return # press [Space] key to enable capturing parent_name = component_instance.robot_parent.blender_obj.name component_name = component_instance.blender_obj.name image = Image() image.header.stamp = rospy.Time.now() image.header.seq = self._seq # http://www.ros.org/wiki/geometry/CoordinateFrameConventions#Multi_Robot_Support image.header.frame_id = ('/' + parent_name + '/base_image') image.height = component_instance.image_height image.width = component_instance.image_width image.encoding = 'rgba8' image.step = image.width * 4 # NOTE: Blender returns the image as a binary string encoded as RGBA # sensor_msgs.msg.Image.image need to be len() friendly # TODO patch ros-py3/common_msgs/sensor_msgs/src/sensor_msgs/msg/_Image.py # to be C-PyBuffer "aware" ? http://docs.python.org/c-api/buffer.html image.data = bytes(image_local.image) # http://wiki.blender.org/index.php/Dev:Source/GameEngine/2.49/VideoTexture # http://www.blender.org/documentation/blender_python_api_2_57_release/bge.types.html#bge.types.KX_Camera.useViewport for topic in self._topics: # publish the message on the correct topic if str(topic.name) == str("/" + parent_name + "/" + component_name + "/image"): topic.publish(image) # sensor_msgs/CameraInfo [ http://ros.org/wiki/rviz/DisplayTypes/Camera ] # TODO fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = component_instance.local_data['intrinsic_matrix'] camera_info = CameraInfo() camera_info.header.stamp = image.header.stamp camera_info.header.seq = image.header.seq camera_info.header.frame_id = image.header.frame_id camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = 'plumb_bob' camera_info.K = [ intrinsic[0][0], intrinsic[1][0], intrinsic[2][0], intrinsic[0][1], intrinsic[1][1], intrinsic[2][1], intrinsic[0][2], intrinsic[1][2], intrinsic[2][2] ] camera_info.R = R camera_info.P = [ intrinsic[0][0], intrinsic[1][0], intrinsic[2][0], Tx, intrinsic[0][1], intrinsic[1][1], intrinsic[2][1], Ty, intrinsic[0][2], intrinsic[1][2], intrinsic[2][2], 0 ] for topic in self._topics: # publish the message on the correct topic if str(topic.name) == str("/" + parent_name + "/" + component_name + "/camera_info"): topic.publish(camera_info) self._seq = self._seq + 1
# Node init and publisher definition rospy.init_node('realsense_rgb_align_depth', anonymous=True) pub_color = rospy.Publisher("rgb_image", Image, queue_size=2) pub_align = rospy.Publisher("align_depth", Image, queue_size=2) pub_camera_info = rospy.Publisher("camera_info", CameraInfo, queue_size=2) rate = rospy.Rate(30) # 30hz # get color camera data profile = pipeline.get_active_profile() color_profile = rs.video_stream_profile(profile.get_stream(rs.stream.color)) color_intrinsics = color_profile.get_intrinsics() camera_info = CameraInfo() camera_info.width = color_intrinsics.width camera_info.height = color_intrinsics.height camera_info.distortion_model = 'plumb_bob' cx = color_intrinsics.ppx cy = color_intrinsics.ppy fx = color_intrinsics.fx fy = color_intrinsics.fy camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] camera_info.D = [0, 0, 0, 0, 0] camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0] camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0] bridge = CvBridge() print("Start node") while not rospy.is_shutdown():
def callback(self, raw_rgb_img): '''Runs depth estimation on live webcam video stream''' #Resize image rgb_img = self.bridge.imgmsg_to_cv2(raw_rgb_img, "bgr8") #544 High x 1024 wide # print type(rgb_img) #np.ndarray # print rgb_img.shape #544x1024x3 img = rgb_img[288:480, :, :] rgb_img = img img = img.reshape(1, 192, 640, 3) img = np.divide(img, 255).astype(np.float16) #Predict depth y_est = self.model.predict(img) y_est = y_est.reshape((192, 640)) #Thresholding for ii, row in enumerate(y_est): for jj, value in enumerate(row): if (y_est[ii][jj]) > 0.02: #max=0.115, min=0.0009 y_est[ii][jj] = 0 y_est = y_est.reshape((192, 640, 1)) #Define ROS messages h = Header() h.stamp = raw_rgb_img.header.stamp #h.stamp=rospy.Time.now() h.frame_id = 'camera_link' #Define depth image message depth_img = self.bridge.cv2_to_imgmsg((y_est * 255).astype(np.float32), "32FC1") depth_img.header = h #Define rgb message rgb_img = self.bridge.cv2_to_imgmsg(rgb_img, "bgr8") rgb_img.header = h #Define camera calibration info. message cal_msg = CameraInfo() cal_msg.header = h cal_msg.distortion_model = "plumb_bob" cal_msg.height = 192 cal_msg.width = 640 #FROM FIELDSAFE MULTISENSE CAMERA #Distortion coefficients cal_msg.D = [ 0.0030753163155168295, 0.002497022273018956, 0.0003005412872880697, 0.001575434347614646, -0.003454494522884488, 0.0, 0.0, 0.0 ] #Intrinsic Camera Matrix cal_msg.K = [ 555.9204711914062, 0.0, 498.1905517578125, 0.0, 556.6275634765625, 252.35089111328125, 0.0, 0.0, 1.0 ] cal_msg.R = [ 0.9999634027481079, -0.000500216381624341, 0.00853759702295065, 0.0005011018947698176, 0.9999998807907104, -0.00010158627264900133, -0.00853754486888647, 0.00010586075950413942, 0.9999635219573975 ] #Projection Matrix cal_msg.P = [ 580.6427001953125, 0.0, 512.0, 0.0, 0.0, 580.6427001953125, 254.5, 0.0, 0.0, 0.0, 1.0, 0.0 ] #Publish messages self.pub.publish(depth_img) self.rgb_pub.publish(rgb_img) self.rgb_pub_cal.publish(cal_msg)
req_width = 320 req_height = 240 capture.set(cv2.CAP_PROP_FRAME_WIDTH, req_width * 2) capture.set(cv2.CAP_PROP_FRAME_HEIGHT, req_height) fr_width = capture.get(cv2.CAP_PROP_FRAME_WIDTH) fr_height = capture.get(cv2.CAP_PROP_FRAME_HEIGHT) fr_width_2 = int(fr_width / 2) print(fr_width, fr_height) bridge = CvBridge() cam_info = CameraInfo() cam_info.height = fr_height cam_info.width = fr_width_2 def main(): while not rospy.is_shutdown(): meta, frame = capture.read() frame_left = frame[:, :fr_width_2, :] frame_right = frame[:, fr_width_2:] # frame_gaus = cv2.GaussianBlur(frame, (3, 3), 0) # frame_gray = cv2.cvtColor(frame_gaus, cv2.COLOR_BGR2GRAY) # I want to publish the Canny Edge Image and the original Image
def project_to_image_plane(self, point_in_world, timestamp): """Project point from 3D world coordinates to 2D camera image location Args: point_in_world (Point): 3D location of a point in the world timestamp (Ros Time): Ros timestamp Returns: x (int): x coordinate of target point in image y (int): y coordinate of target point in image """ camera_info = CameraInfo() fx = self.config['camera_info']['focal_length_x'] fy = self.config['camera_info']['focal_length_y'] camera_info.width = self.config['camera_info']['image_width'] camera_info.height = self.config['camera_info']['image_height'] #print("fx {}, fy {}".format(fx, fy)) camera_info.K = np.array([[fx, 0, camera_info.width / 2], [0, fy, camera_info.height / 2], [0, 0, 1.]], dtype=np.float32) camera_info.P = np.array([[fx, 0, camera_info.width / 2, 0], [0, fy, camera_info.height / 2, 0], [0, 0, 1., 0]]) camera_info.R = np.array([[1., 0, 0], [0, 1., 0], [0, 0, 1.]], dtype=np.float32) camera = PinholeCameraModel() camera.fromCameraInfo(camera_info) #print("point_in_world = {}".format(str(point_in_world))) #print("camera projection matrix ", camera.P) # get transform between pose of camera and world frame trans = None point_in_camera_space = None point_in_image = None bbox_points_camera_image = [] euler_transforms = ( math.radians(90), # roll along X to force Y axis 'up' math.radians( -90 + -.75 ), # pitch along Y to force X axis towards 'right', with slight adjustment for camera's 'yaw' math.radians( -9 ) # another roll to orient the camera slightly 'upwards', (camera's 'pitch') ) euler_axes = 'sxyx' try: self.listener.waitForTransform("/base_link", "/world", timestamp, rospy.Duration(0.1)) (trans, rot) = self.listener.lookupTransform("/base_link", "/world", timestamp) camera_orientation_adj = tf.transformations.quaternion_from_euler( *euler_transforms, axes=euler_axes) trans_matrix = self.listener.fromTranslationRotation(trans, rot) camera_orientation_adj = self.listener.fromTranslationRotation( (0, 0, 0), camera_orientation_adj) #print("trans {}, rot {}".format(trans, rot)) #print("transform matrix {}".format(trans_matrix)) point = np.array( [point_in_world.x, point_in_world.y, point_in_world.z, 1.0]) # this point should match what you'd see from being inside the vehicle looking straight ahead. point_in_camera_space = trans_matrix.dot(point) #print("point in camera frame {}".format(point_in_camera_space)) final_trans_matrix = camera_orientation_adj.dot(trans_matrix) # this point is from the view point of the camera (oriented along the camera's rotation quaternion) point_in_camera_space = final_trans_matrix.dot(point) #print("point in camera frame adj {}".format(point_in_camera_space)) bbox_points = [ (point_in_camera_space[0] - 0.5, point_in_camera_space[1] - 1.1, point_in_camera_space[2], 1.0), (point_in_camera_space[0] + 0.5, point_in_camera_space[1] + 1.1, point_in_camera_space[2], 1.0), (point_in_camera_space[0] - 0.5, point_in_camera_space[1] - 1.1, point_in_camera_space[2], 1.0), (point_in_camera_space[0] + 0.5, point_in_camera_space[1] + 1.1, point_in_camera_space[2], 1.0) ] # these points represent the bounding box within the camera's image for p in bbox_points: bbox_points_camera_image.append(camera.project3dToPixel(p)) # print("point in image {}".format(bbox_points_camera_image)) except (tf.Exception, tf.LookupException, tf.ConnectivityException): rospy.logerr("Failed to find camera to map transform") return bbox_points_camera_image
def step(self): t = rospy.Time.now() _, _, oldwidth, oldheight = glGetFloatv(GL_VIEWPORT) height = min(oldheight, int(oldwidth / self.aspect + .5)) width = int(self.aspect * height + .5) glViewport(0, 0, width, height) msg = CameraInfo() msg.header.stamp = t msg.header.frame_id = '/' + self.name msg.height = height msg.width = width f = 1 / math.tan(math.radians(self.fovy) / 2) * height / 2 msg.P = [ f, 0, width / 2 - .5, 0, 0, f, height / 2 - .5, 0, 0, 0, 1, 0, ] self.info_pub.publish(msg) glMatrixMode(GL_PROJECTION) glLoadIdentity() perspective(self.fovy, width / height, 0.1) glMatrixMode(GL_MODELVIEW) glLoadIdentity() # rotates into the FLU coordinate system glMultMatrixf([[0., 0., -1., 0.], [-1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]]) # after that, +x is forward, +y is left, and +z is up self.set_pose_func() with GLMatrix: rotate_to_body(self.base_link_body) glcamera_from_body = glGetFloatv(GL_MODELVIEW_MATRIX).T camera_from_body = numpy.array([ # camera from glcamera [1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1], ]).dot(glcamera_from_body) body_from_camera = numpy.linalg.inv(camera_from_body) tf_br.sendTransform( transformations.translation_from_matrix(body_from_camera), transformations.quaternion_from_matrix(body_from_camera), t, "/" + self.name, "/base_link") if not self.image_pub.get_num_connections(): glViewport(0, 0, oldwidth, oldheight) return self.world.draw() x = glReadPixels(0, 0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, outputType=None) x = numpy.reshape(x, (height, width, 4)) x = x[::-1] msg = Image() msg.header.stamp = t msg.header.frame_id = '/' + self.name msg.height = height msg.width = width msg.encoding = 'rgba8' msg.is_bigendian = 0 msg.step = width * 4 msg.data = x.tostring() self.image_pub.publish(msg) glViewport(0, 0, oldwidth, oldheight)
def publish(self, incomingData): if "apriltags" in incomingData: for tag in incomingData["apriltags"]: # Publish the relative pose newApriltagDetectionMsg = AprilTagExtended() newApriltagDetectionMsg.header.stamp.secs = int( tag["timestamp_secs"]) newApriltagDetectionMsg.header.stamp.nsecs = int( tag["timestamp_nsecs"]) newApriltagDetectionMsg.header.frame_id = str(tag["source"]) newApriltagDetectionMsg.transform.translation.x = float( tag["tvec"][0]) newApriltagDetectionMsg.transform.translation.y = float( tag["tvec"][1]) newApriltagDetectionMsg.transform.translation.z = float( tag["tvec"][2]) newApriltagDetectionMsg.transform.rotation.x = float( tag["qvec"][0]) newApriltagDetectionMsg.transform.rotation.y = float( tag["qvec"][1]) newApriltagDetectionMsg.transform.rotation.z = float( tag["qvec"][2]) newApriltagDetectionMsg.transform.rotation.w = float( tag["qvec"][3]) newApriltagDetectionMsg.tag_id = int(tag["tag_id"]) newApriltagDetectionMsg.tag_family = tag["tag_family"] newApriltagDetectionMsg.hamming = int(tag["hamming"]) newApriltagDetectionMsg.decision_margin = float( tag["decision_margin"]) newApriltagDetectionMsg.homography = tag["homography"].flatten( ) newApriltagDetectionMsg.center = tag["center"] newApriltagDetectionMsg.corners = tag["corners"].flatten() newApriltagDetectionMsg.pose_error = tag["pose_error"] self.publish_queue.put({ "topic": "apriltags", "message": newApriltagDetectionMsg }) # self.publishers["apriltags"].publish(newApriltagDetectionMsg) # self.logger.info("Published pose for tag %d in sequence %d" % ( # tag["tag_id"], self.seq_stamper)) # Publish the test and raw data if submitted and requested: if self.ACQ_TEST_STREAM: if "test_stream_image" in incomingData: imgMsg = incomingData["test_stream_image"] imgMsg.header.seq = self.seq_stamper self.publish_queue.put({ "topic": "test_stream_image", "message": imgMsg }) # self.publishers["test_stream_image"].publish(imgMsg) if "raw_image" in incomingData: imgMsg = incomingData["raw_image"] imgMsg.header.seq = self.seq_stamper self.publish_queue.put({ "topic": "raw_image", "message": imgMsg }) # self.publishers["raw_image"].publish(imgMsg) if "rectified_image" in incomingData: imgMsg = incomingData["rectified_image"] imgMsg.header.seq = self.seq_stamper self.publish_queue.put({ "topic": "rectified_image", "message": imgMsg }) # self.publishers["rectified_image"].publish(imgMsg) if "raw_camera_info" in incomingData: self.publish_queue.put({ "topic": "raw_camera_info", "message": incomingData["raw_camera_info"] }) # self.publishers["raw_camera_info"].publish( # incomingData["raw_camera_info"]) if "new_camera_matrix" in incomingData: new_camera_info = CameraInfo() try: new_camera_info.header = incomingData[ "raw_camera_info"].header new_camera_info.height = incomingData["raw_image"].shape[0] new_camera_info.width = incomingData["raw_image"].shape[1] new_camera_info.distortion_model = incomingData[ "raw_camera_info"].distortion_model new_camera_info.D = [0.0, 0.0, 0.0, 0.0, 0.0] except: pass new_camera_info.K = incomingData["new_camera_matrix"].flatten() self.publish_queue.put({ "topic": "new_camera_matrix", "message": new_camera_info })
value_str = line[line.find('[') + 1:line.find(']')] value = [float(s) for s in value_str.split(',')] if camera_idx == 1: camera_info_1.P = value else: camera_info_2.P = value elif line.startswith('width'): need_width = True elif line.startswith('height'): need_height = True elif need_width: camera_info_1.width = int(line) camera_info_2.width = int(line) need_width = False elif need_height: camera_info_1.height = int(line) camera_info_2.height = int(line) need_height = False output_path = filename[:filename.rfind('/')] + '/output/' output_file = filename[filename.rfind('/') + 1:] if not os.path.exists(output_path): os.makedirs(output_path) if stereo: with open(output_path + output_file + '_left_intrinsics.yaml', 'w') as of: of.write(str(camera_info_1)) with open(output_path + output_file + '_right_intrinsics.yaml', 'w') as of: of.write(str(camera_info_2))
def grabAndPublish(self, publisher): # Grab image from simulation and apply the action obs, reward, done, info = self.env.step(self.action) #rospy.loginfo('[%s] step_count = %s, reward=%.3f' % (self.node_name, self.env.unwrapped.step_count, reward)) image = cv2.cvtColor(obs, cv2.COLOR_BGR2RGB) # Correct color for cv2 """ ##show image cv2.namedWindow("Image") if (not image is None): cv2.imshow("Image",image) if cv2.waitKey(1)!=-1: #Burak, needs to modify this line to work on your computer, THANKS! cv2.destroyAllWindows() """ # Publish raw image image_msg = self.bridge.cv2_to_imgmsg(image, "bgr8") image_msg.header.stamp = rospy.Time.now() # Publish publisher.publish(image_msg) cam_msg = CameraInfo() cam_msg.height = 480 cam_msg.width = 640 #cam_msg.height = 240 #cam_msg.width= 320 cam_msg.header.stamp = image_msg.header.stamp cam_msg.header.frame_id = 'cam' cam_msg.distortion_model = 'plumb_bob' cam_msg.D = [ -0.2, 0.0305, 0.0005859930422629722, -0.0006697840226199427, 0 ] cam_msg.K = [ 305.5718893575089, 0, 303.0797142544728, 0, 308.8338858195428, 231.8845403702499, 0, 0, 1, ] cam_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] cam_msg.P = [ 220.2460277141687, 0, 301.8668918355899, 0, 0, 238.6758484095299, 227.0880056118307, 0, 0, 0, 1, 0, ] self.pub_cam.publish(cam_msg) if not self.has_published: rospy.loginfo("[%s] Published the first image." % (self.node_name)) self.has_published = True #if done and (self.env.unwrapped.step_count != 1500): #rospy.logwarn("[%s] DONE! RESETTING." %(self.node_name)) #self.env.reset() self.env.render()
from sensor_msgs.msg import CameraInfo #from __future__ import absolute_import, print_function, division from pymba import * import numpy as np import cv2 import time from cv_bridge import CvBridge, CvBridgeError #cv2.namedWindow("test") with Vimba() as vimba: #camera info camera_info_msg = CameraInfo() camera_info_msg.header.frame_id = "avt_manta" camera_info_msg.width = 1624 camera_info_msg.height = 1234 camera_info_msg.K = [ 1792.707269, 0.0, 821.895887, 0.0, 1790.871098, 624.859714, 0.0, 0.0, 1.0 ] camera_info_msg.D = [-0.225015, 0.358593, -0.005422, 0.009070, 0.0] camera_info_msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] camera_info_msg.P = [ 1736.461670, 0.0, 834.094334, 0.0, 0.0, 1751.635254, 621.266132, 0.0, 0.0, 0.0, 1.0, 0.0 ] camera_info_msg.distortion_model = "narrow_stereo" ########## system = vimba.getSystem()
def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic, initial_time): print("Exporting camera {}".format(camera)) if kitti_type.find("raw") != -1: camera_pad = '{0:02d}'.format(camera) image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad)) image_path = os.path.join(image_dir, 'data') image_filenames = sorted(os.listdir(image_path)) with open(os.path.join(image_dir, 'timestamps.txt')) as f: image_datetimes = map( lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines()) calib = CameraInfo() calib.header.frame_id = camera_frame_id calib.width, calib.height = tuple( util['S_rect_{}'.format(camera_pad)].tolist()) calib.distortion_model = 'plumb_bob' calib.K = util['K_{}'.format(camera_pad)] calib.R = util['R_rect_{}'.format(camera_pad)] calib.D = util['D_{}'.format(camera_pad)] calib.P = util['P_rect_{}'.format(camera_pad)] elif kitti_type.find("odom") != -1: camera_pad = '{0:01d}'.format(camera) image_path = os.path.join(kitti.sequence_path, 'image_{}'.format(camera_pad)) image_filenames = sorted(os.listdir(image_path)) image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps) calib = CameraInfo() calib.header.frame_id = camera_frame_id #calib.P = util['P{}'.format(camera_pad)] calib.distortion_model = 'plumb_bob' if camera == 0: calib.K = kitti.calib.K_cam0.flatten() #calib.P = kitti.calib.P_rect_00.flatten() iterable = zip(image_datetimes, image_filenames) for dt, filename in tqdm.tqdm(iterable, total=len(image_filenames)): image_filename = os.path.join(image_path, filename) cv_image = cv2.imread(image_filename) calib.height, calib.width = cv_image.shape[:2] if camera in (0, 1): cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) encoding = "mono8" if camera in (0, 1) else "bgr8" image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding) image_message.header.frame_id = camera_frame_id image_message.header.seq = calib.header.seq calib.header.seq += 1 if kitti_type.find("raw") != -1: image_message.header.stamp = rospy.Time.from_sec( float(datetime.strftime(dt, "%s.%f"))) topic_ext = "/image" elif kitti_type.find("odom") != -1: image_message.header.stamp = rospy.Time.from_sec(dt) topic_ext = "/image" calib.header.stamp = image_message.header.stamp bag.write(topic + topic_ext, image_message, t=image_message.header.stamp) bag.write(topic + '/camera_info', calib, t=calib.header.stamp)
rospy.Subscriber("image_raw", Image, callback) # spin() simply keeps python from exiting until this node is stopped rospy.spin() if __name__ == '__main__': rospy.init_node('pub_camera_info_node', anonymous=True) calib_file = rospy.get_param('~calib') with file(calib_file, 'r') as f: params = yaml.load(f) cam_info.height = params['cam0']['resolution'][1] cam_info.width = params['cam0']['resolution'][0] cam_info.distortion_model = 'plumb_bob' # cam_info.K = params['K'] # cam_info.P = params['P'] cam_info.D = params['cam0']['distortion_coeffs'] cam_info.P = [0] * 12 cam_info.R = [0] * 9 cam_info.K = [0] * 9 cam_info.K[0] = params['cam0']['intrinsics'][0] cam_info.K[2] = params['cam0']['intrinsics'][2] cam_info.K[4] = params['cam0']['intrinsics'][1] cam_info.K[5] = params['cam0']['intrinsics'][3] cam_info.K[8] = 1
def default(self, ci='unused'): """ Publish the data of the Camera as a ROS Image message. """ if not self.component_instance.capturing: return # press [Space] key to enable capturing image_local = self.data['image'] image = Image() image.header = self.get_ros_header() image.header.frame_id += '/base_image' image.height = self.component_instance.image_height image.width = self.component_instance.image_width image.encoding = 'rgba8' image.step = image.width * 4 # VideoTexture.ImageRender implements the buffer interface image.data = bytes(image_local) # sensor_msgs/CameraInfo [ http://ros.org/wiki/rviz/DisplayTypes/Camera ] # fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = self.data['intrinsic_matrix'] camera_info = CameraInfo() camera_info.header = image.header camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = 'plumb_bob' camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]] camera_info.R = R camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx, intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty, intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0] self.publish(image) self.topic_camera_info.publish(camera_info)