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)
