class gui_calibration(QtCore.QObject):
def __init__(self, scene, img_path, size):
super(gui_calibration, self).__init__()
self.img_path = img_path
self.scene = scene
self.checkerboard_img = QtGui.QPixmap(self.img_path + "/pattern.png")
self.checkerboard = self.scene.addPixmap(self.checkerboard_img.scaled(size, QtCore.Qt.KeepAspectRatio))
self.checkerboard.setZValue(100)
self.checkerboard.hide()
self.bridge = CvBridge()
self.calibrated_pub = rospy.Publisher("~calibrated", Bool, queue_size=10, latch=True)
self.corners_pub = rospy.Publisher("~corners", PoseArray, queue_size=10, latch=True)
self.srv_calibrate = rospy.Service('~calibrate', Empty, self.calibrate)
self.size = size
self.model = None
self.h_matrix = None
try:
s = rospy.get_param("~calibration_matrix")
self.h_matrix = np.matrix(ast.literal_eval(s))
rospy.loginfo("Loaded calibration from param server")
except KeyError:
pass
self.calibrated_pub.publish(Bool(self.h_matrix is not None))
QtCore.QObject.connect(self, QtCore.SIGNAL('calibrate'), self.calibrate_evt)
QtCore.QObject.connect(self, QtCore.SIGNAL('calibrate2'), self.calibrate_evt2)
self.on_request = None
self.on_finished = None
def resize(self, size):
self.size = size
self.checkerboard.setPixmap(self.checkerboard_img.scaled(size, QtCore.Qt.KeepAspectRatio))
def is_calibrated(self):
return self.h_matrix is not None
def get_px(self, pos):
if self.h_matrix is None:
rospy.logerr("Not calibrated!")
return None
if isinstance(pos, Pose):
psx = pos.position.x
psy = pos.position.y
elif isinstance(pos, PointStamped):
psx = pos.point.x
psy = pos.point.y
elif isinstance(pos, Point32):
psx = pos.x
psy = pos.y
pt = np.array([[psx], [psy], [1.0]])
px = self.h_matrix*pt
w = px[2].tolist()[0][0]
x = px[0].tolist()[0][0]
y = px[1].tolist()[0][0]
return (self.size.width()-int(round(x/w)), int(round(y/w)))
def get_pose(self, px, py):
ps = PoseStamped()
ps.header.frame_id = "marker"
ps.header.stamp = rospy.Time.now()
p = np.array([[self.size.width()-px], [py], [1.0]])
res = np.linalg.inv(self.h_matrix)*p
ps.pose.position.x = float(res[0]/res[2])
ps.pose.position.y = float(res[1]/res[2])
ps.pose.position.z = 0.0
ps.pose.orientation.x = 0.0
ps.pose.orientation.y = 0.0
ps.pose.orientation.z = 0.0
ps.pose.orientation.w = 1.0
return ps
def get_point32(self, px, py):
ps = Point32()
p = np.array([[self.size.width()-px], [py], [1.0]])
res = np.linalg.inv(self.h_matrix)*p
ps.x = float(res[0]/res[2])
ps.y = float(res[1]/res[2])
ps.z = 0.0
return ps
def get_caminfo(self):
cam_info = None
try:
cam_info = rospy.wait_for_message('camera_info', CameraInfo, 1.0)
except rospy.ROSException:
rospy.logerr("Could not get camera_info")
if cam_info is not None:
self.model = PinholeCameraModel()
self.model.fromCameraInfo(cam_info)
def calibrate(self, req):
self.tfl = tf.TransformListener()
# TODO subscribe to image/depth (message_filters?) and call calibrate_evt2 from there
self.emit(QtCore.SIGNAL('calibrate'))
if self.on_request is not None: self.on_request()
return EmptyResponse()
def calibrate_evt(self):
self.checkerboard.show()
self.ctimer = QtCore.QTimer.singleShot(1000, self.calibrate_evt2)
def calibrate_int(self):
points = []
ppoints = []
cnt = 0
box_size = self.checkerboard.pixmap().width()/(10+2.0) # in pixels
origin = (2*box_size, 2*box_size) # origin of the first corner
ppp = PoseArray()
ppp.header.stamp = rospy.Time.now()
ppp.header.frame_id = "marker"
while(cnt < 3):
cnt += 1
try:
img = rospy.wait_for_message('camera_image', Image, 1.0)
except rospy.ROSException:
rospy.logerr("Could not get image")
return False
try:
depth = rospy.wait_for_message('camera_depth_image', Image, 1.0)
except rospy.ROSException:
rospy.logerr("Could not get depth image")
return False
rospy.loginfo("Got data...")
try:
cv_img = self.bridge.imgmsg_to_cv2(img, "bgr8")
except CvBridgeError as e:
print(e)
return False
try:
cv_depth = self.bridge.imgmsg_to_cv2(depth)
except CvBridgeError as e:
print(e)
return False
cv_depth = cv2.medianBlur(cv_depth, 5)
cv_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(cv_img, (9,6), None, flags=cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_FILTER_QUADS | cv2.CALIB_CB_NORMALIZE_IMAGE)
if ret == False:
rospy.logerr("Could not find chessboard corners")
return False
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
cv2.cornerSubPix(cv_img,corners,(11,11),(-1,-1),criteria)
corners = corners.reshape(1,-1,2)[0]
# take chessboard corners and make 3D points using projection and depth data
for c in corners:
pt = list(self.model.projectPixelTo3dRay((c[0], c[1])))
pt[:] = [x/pt[2] for x in pt]
# depth image is noisy - let's make mean of few pixels
da = []
for x in range(int(c[0]) - 2, int(c[0]) + 3):
for y in range(int(c[1]) - 2, int(c[1]) + 3):
da.append(cv_depth[y, x]/1000.0)
d = np.mean(da)
pt[:] = [x*d for x in pt]
ps = PointStamped()
ps.header.stamp = rospy.Time(0)
ps.header.frame_id = img.header.frame_id
ps.point.x = pt[0]
ps.point.y = pt[1]
ps.point.z = pt[2]
# transform 3D point from camera into the world coordinates
try:
ps = self.tfl.transformPoint("marker", ps)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logerr("can't get transform")
return False
pp = Pose()
pp.position.x = ps.point.x
pp.position.y = ps.point.y
pp.position.z = ps.point.z
pp.orientation.x = 0
pp.orientation.y = 0
pp.orientation.z = 0
pp.orientation.w = 1.0
ppp.poses.append(pp)
# store x,y -> here we assume that points are 2D (on tabletop)
points.append([ps.point.x, ps.point.y])
# generate 2D (screen) points of chessboard corners (in pixels)
for y in range(0,6):
for x in range(0,9):
px = self.size.width()-(origin[0]+x*box_size)
py = (origin[1]+y*box_size)
ppoints.append([px, py])
self.corners_pub.publish(ppp)
# find homography between points on table (in meters) and screen points (pixels)
h, status = cv2.findHomography(np.array(points), np.array(ppoints), cv2.LMEDS)
self.h_matrix = np.matrix(h)
self.box_size = box_size
self.pm_width = self.checkerboard.pixmap().width()
rospy.loginfo("Calibrated!")
# store homography matrix to parameter server
s = str(self.h_matrix.tolist())
rospy.set_param("~calibration_matrix", s)
print s
return True
def calibrate_evt2(self):
cnt = 0
if self.model is None:
self.get_caminfo()
if self.model is None:
rospy.logerr("No camera_info -> cannot calibrate")
cnt = 10
ret = False
while ret == False and cnt < 5:
ret = self.calibrate_int()
cnt += 1
self.checkerboard.hide()
self.tfl = None
if self.on_finished is not None: self.on_finished()
self.calibrated_pub.publish(Bool(ret))
def FindDist(color):
cont = checkIfColorIsShowed(color)
if cont == []:
rospy.sleep(0.5)
print "None"
return None
cnt = cont[0]
M = cv2.moments(cnt)
center_x = int(M['m10'] / M['m00'])
center_y = 400
imgCntr = (400, 400)
camInfo = rospy.wait_for_message("camera/camera_info", CameraInfo)
camera = PinholeCameraModel()
camera.fromCameraInfo(camInfo)
cenRay = camera.projectPixelTo3dRay(imgCntr)
pixRay = camera.projectPixelTo3dRay((center_x, center_y))
angle = math.acos(
prod(pixRay, cenRay) / (math.sqrt(prod(pixRay, pixRay))) *
(math.sqrt(prod(cenRay, cenRay))))
dg = int(math.degrees(angle))
if center_x >= imgCntr[0]:
dg = 360 - dg
LaserMessage = rospy.wait_for_message("scan", LaserScan)
return retrunResult(LaserMessage, dg)
def dpx_to_distance(dx, dy, camera_name, current_ps, offset):
print("The dx is " + str(dx) + " the dy is " + str(dy) + " and the camera name is " + camera_name)
big_z_mappings = {'camera1': transform_broadcaster_mapping['camera1'][0][2] - current_ps.pose.position.z, # x-y plane
'camera2': transform_broadcaster_mapping['camera2'][0][1] - current_ps.pose.position.y, # x-z plane
'camera3': transform_broadcaster_mapping['camera3'][0][0] - current_ps.pose.position.x} # y-z plane
#print("The frame_id for the current pose is " + current_ps.header.frame_id)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info_mapping[camera_name])
x, y, z = camera_model.projectPixelTo3dRay((dx, dy))
#print " x : {} , y : {} , z : {}".format(x, y, z)
x_center, y_center, z_center = camera_model.projectPixelTo3dRay((0, 0))
big_z = abs(big_z_mappings[camera_name])
#print("The big_z for " + camera_name + " is " + str(big_z))
big_x = (x / z) * big_z # Use law of similar trianges to solve
big_y = (y / z) * big_z
big_x_center = (x_center / z_center) * big_z
big_y_center = (y_center / z_center) * big_z
#print("The x_center is " + str(x_center) + " the y_center is " + str(y_center) + " and the z_center is " + str(
# z_center))
#print(
#"The x distance is " + str(big_x - big_x_center) + " the y distance is " + str(big_y - big_y_center) + " and the camera name is " + camera_name + "\n")
if offset:
return big_x - big_x_center, big_y - big_y_center
else:
return big_x, big_y
class CamPixelToPointServer:
def __init__(self):
self.camera_model = PinholeCameraModel()
self.bridge = CvBridge()
self.camera_model.fromCameraInfo(SubscriberValue('camera_info', CameraInfo).value)
self.depth_image = SubscriberValue('camera_depth_image', Image, transform=self.bridge.imgmsg_to_cv2)
self.service = rospy.Service('cam_pixel_to_point', CamPixelToPoint, self.handle_service)
self.tf_listener = TransformListener()
print('Service is ready.')
def handle_service(self, req): # type: (CamPixelToPoint) -> CamPixelToPointResponse
x, y = int(req.cam_pixel.x), int(req.cam_pixel.y)
methods = [self.read_depth_simple,
# self.read_depth_average,
self.read_depth_as_floor_depth]
for method in methods:
d = method(x, y)
if not np.isnan(d):
break
pos = np.array(self.camera_model.projectPixelTo3dRay((x, y))) * d
point = PointStamped()
point.header.frame_id = self.camera_model.tfFrame()
point.point.x, point.point.y, point.point.z = pos[0], pos[1], pos[2]
return CamPixelToPointResponse(point)
def read_depth_simple(self, x, y): # (int, int) -> float
return self.depth_image.value[y, x]
def read_depth_average(self, x, y): # (int, int) -> float
print('Fallback to average')
s = 5
return np.nanmean(self.depth_image.value[y-s:y+s, x-s:x+s])
def read_depth_as_floor_depth(self, x, y): # (int, int) -> float
print('Fallback to floor model')
min_distance = 10.0
# Extend the camera ray until it passes through where the floor should be. Use its length as the depth.
camera_origin = PointStamped()
camera_origin.header.frame_id = self.camera_model.tfFrame()
camera_origin.point.x, camera_origin.point.y, camera_origin.point.z = 0.0, 0.0, 0.0
point_along_ray = PointStamped()
point_along_ray.header.frame_id = self.camera_model.tfFrame()
point_along_ray.point.x, point_along_ray.point.y, point_along_ray.point.z = self.camera_model.projectPixelTo3dRay((x, y))
self.tf_listener.waitForTransform('base_footprint', self.camera_model.tfFrame(), rospy.Time(rospy.get_time()), rospy.Duration(1))
camera_origin = self.tf_listener.transformPoint('base_footprint', camera_origin)
point_along_ray = self.tf_listener.transformPoint('base_footprint', point_along_ray)
camera_origin = np_from_poinstamped(camera_origin)
point_along_ray = np_from_poinstamped(point_along_ray)
ray_dir = point_along_ray - camera_origin
# Assuming this transformation was orthogonal, |ray_dir| = 1, at least approximately
d = camera_origin[1]/max(-ray_dir[1], camera_origin[1]/min_distance)
if d <= 0.01:
d = np.nan
return d
class PointFromPixel():
""" Given a pixel location, find its 3D location in the world """
def __init__(self, topic_camera_info, topic_depth):
self.need_camera_info = True
self.need_depth_image = True
self.model = PinholeCameraModel()
rospy.Subscriber(topic_camera_info, CameraInfo, self.callback_camera_info)
rospy.Subscriber(topic_depth, Image, self.callback_depth_image)
def callback_camera_info(self, info):
""" Define Pinhole Camera Model parameters using camera info msg """
if self.need_camera_info:
rospy.loginfo('Got camera info!')
self.model.fromCameraInfo(info) # define model params
self.frame = info.header.frame_id
self.need_camera_info = False
def callback_depth_image(self, depth_image):
""" Get depth at chosen pixel using depth image """
if self.need_depth_image:
rospy.loginfo('Got depth image!')
self.depth = img.fromstring("F", (depth_image.width, depth_image.height), depth_image.data)
self.need_depth_image = False
def calculate_3d_point(self, pixel):
""" Project ray through chosen pixel, then use pixel depth to get 3d point """
lookup = self.depth.load()
depth = lookup[pixel[0], pixel[1]] # lookup pixel in depth image
ray = self.model.projectPixelTo3dRay(tuple(pixel)) # get 3d ray of unit length through desired pixel
ray_z = [el / ray[2] for el in ray] # normalize the ray so its Z-component equals 1.0
pt = [el * depth for el in ray_z] # multiply the ray by the depth; its Z-component should now equal the depth value
point = PointStamped()
point.header.frame_id = self.frame
point.point.x = pt[0]
point.point.y = pt[1]
point.point.z = pt[2]
return point
def ray_plane_intersection(self, pixel, plane):
""" Given plane parameters [a, b, c, d] as in ax+by+cz+d=0,
finds intersection of 3D ray with the plane. """
ray = self.model.projectPixelTo3dRay(tuple(pixel)) # get 3d ray of unit length through desired pixel
scale = -plane[3] / (plane[0]*ray[0] + plane[1]*ray[1] + plane[2]*ray[2])
point = PointStamped()
point.header.frame_id = self.frame
point.point.x = ray[0] * scale
point.point.y = ray[1] * scale
point.point.z = ray[2] * scale
return point
def broadcast_poses():
robot = robot_interface.Robot_Interface()
cam = camera.RGBD()
not_read = True
while not_read:
try:
cam_info = cam.read_info_data()
if (not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
pcm = PCM()
pcm.fromCameraInfo(cam_info)
point = (208 + (323 - 208) / 2, 247 + (424 - 247) / 2)
print(point)
#(208.076538,247.264099) (323.411957,242.667325) (204.806457,424.053619) (324.232857,434.011618)
dep_data = robot.get_img_data()[1]
print(dep_data)
dep = robot.get_depth(point, dep_data)
print(dep)
br = tf.TransformBroadcaster()
td_points = pcm.projectPixelTo3dRay(point)
# pose = np.array([td_points[0], td_points[1], 0.001 * dep])
norm_pose = np.array(td_points)
norm_pose = norm_pose / norm_pose[2]
norm_pose = norm_pose * (dep)
a = tf.transformations.quaternion_from_euler(ai=-2.355, aj=-3.14, ak=0.0)
b = tf.transformations.quaternion_from_euler(ai=0.0, aj=0.0, ak=1.57)
base_rot = tf.transformations.quaternion_multiply(a, b)
br.sendTransform(norm_pose, base_rot, rospy.Time.now(), 'tree',
'head_camera_rgb_optical_frame')
class RGBD_Project(object):
def __init__(self):
self.cam = RGBD()
not_read = True
while not_read:
try:
cam_info = self.cam.read_info_data()
if (not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
self.pcm.fromCameraInfo(cam_info)
def deproject_pose(self, pose):
""""
pose = (u_x,u_y,z)
u_x,u_y correspond to pixel value in image
x corresponds to depth
"""
td_points = self.pcm.projectPixelTo3dRay((pose[0], pose[1]))
norm_pose = np.array(td_points)
norm_pose = norm_pose / norm_pose[2]
norm_pose = norm_pose * (cfg.MM_TO_M * pose[2])
return norm_pose
def centroid_to_pose(self, rect, moment):
"""
Publish the region of interest as a geometry_msgs/Pose message from
the ROI or the center of mass coordinates
@param rect - sensor_msgs/RegionOfInterest
@param moment - object's center of mass
"""
if not moment:
moment = (int(rect[0] + rect[2] / 2), int(rect[1] + rect[3] / 2))
u, v = int(moment[0]), int(moment[1])
ps = Pose()
ps.orientation.x = 0.0
ps.orientation.y = 0.0
ps.orientation.z = 0.0
ps.orientation.w = 1.0
cam = PinholeCameraModel()
cam.fromCameraInfo(self.cam_info)
(x, y, z) = cam.projectPixelTo3dRay((u, v))
ps.position.x = x
ps.position.y = y
ps.position.z = z
return ps
class CamPixelToPointServer:
def __init__(self):
self.camera_model = PinholeCameraModel()
self.bridge = CvBridge()
self.model_set = False
self.tf_listener = TransformListener()
def pixel_to_point(self,
cam_pos,
out_frame='map'): # type: (np.ndarray) -> PointStamped
if not self.model_set:
self.camera_model.fromCameraInfo(
rospy.wait_for_message('/camera/rgb/camera_info', CameraInfo))
self.model_set = True
x, y = int(cam_pos[0]), int(cam_pos[1])
d = self.read_depth_simple(x, y)
pos = np.array(self.camera_model.projectPixelTo3dRay((x, y))) * d
point = PointStamped()
point.header.frame_id = self.camera_model.tfFrame()
point.point.x, point.point.y, point.point.z = pos[0], pos[1], pos[2]
point = convert_frame(self.tf_listener, point, out_frame)
return point
def read_depth_simple(self, x, y): # (int, int) -> float
image = rospy.wait_for_message('/camera/depth_registered/image_raw',
Image)
image = self.bridge.imgmsg_to_cv2(image)
return image[y, x]
class DrawRect(): def __init__(self, topic_image, topic_blob, topic_info, topic_out): self.sync1_callback = False #synchronize the image self.sync2_callback = False #synchronize the camera info self.bridge = CvBridge() rospy.Subscriber(topic_image, Image, self.image_callback) rospy.Subscriber(topic_blob, Blobs, self.blob_callback) rospy.Subscriber(topic_info, CameraInfo, self.info_callback) self.pub = rospy.Publisher(topic_out, PointStamped) def image_callback(self, image): self.sync1_callback = True self.image = self.bridge.imgmsg_to_cv(image) self.image = np.asarray(self.image) def info_callback (self, info): #get the camera information self.sync2_callback = True self.info = info def blob_callback(self, blob): if self.sync1_callback and self.sync2_callback and (blob.blob_count != 0): bleb = self.FindBiggestBlob (blob) z = self.image[bleb.y][bleb.x] # depth in mm (INT!) z /= 1000.0 # now in meters (FLOAT!) self.UVtoXYZ = PinholeCameraModel() self.UVtoXYZ.fromCameraInfo(self.info) vec = self.UVtoXYZ.projectPixelTo3dRay ((bleb.x, bleb.y)) vec = [x * (z/vec[2]) for x in vec] self.P = PointStamped() self.P.header.frame_id = self.info.header.frame_id self.P.point.x = vec[0] #the np make the downward direction positive direction, #so we need to multiply to -1 to make upward direction #positive direction self.P.point.y = vec[1] self.P.point.z = vec[2] print self.P self.pub.publish(self.P) def FindBiggestBlob(self, blob): #Assume the hat is the only green blob in front of the camera x = 0 bleb = None while x < blob.blob_count: if (x == 0): bleb = blob.blobs[x] else: if (blob.blobs[x].area > bleb.area): bleb = blob.blobs[x] x = x + 1 return bleb
def get_rays(self):
model = PinholeCameraModel()
model.fromCameraInfo(self.depth_info)
self.array_rays = numpy.zeros((self.image_depth.height, self.image_depth.width, 3))
for u in range(self.image_depth.height):
for v in range(self.image_depth.width):
ray = model.projectPixelTo3dRay((u, v))
ray_z = [el / ray[2] for el in ray] # normalize the ray so its Z-component equals 1.0
self.array_rays[u, v, :] = ray_z
def make_pixel_to_3d(self):
cm = PinholeCameraModel()
cm.fromCameraInfo(self.camera_info)
self.p3d = np.zeros(
[self.camera_info.height, self.camera_info.width, 3])
for v in range(0, self.camera_info.height):
for u in range(0, self.camera_info.width):
uv_rect = cm.rectifyPoint((u, v))
p = cm.projectPixelTo3dRay(uv_rect)
self.p3d[v, u, :] = (p[0] / p[2], p[1] / p[2], 1.0
) # make homogeneous
def get_rays(self):
model = PinholeCameraModel()
model.fromCameraInfo(self.depth_info)
self.array_rays = numpy.zeros(
(self.image_depth.height, self.image_depth.width, 3))
for u in range(self.image_depth.height):
for v in range(self.image_depth.width):
ray = model.projectPixelTo3dRay((u, v))
ray_z = [el / ray[2] for el in ray
] # normalize the ray so its Z-component equals 1.0
self.array_rays[u, v, :] = ray_z
def PointFromPixel(pixel, frame):
model = PinholeCameraModel()
depth = 10000 #valeur arbitraire
ray = model.projectPixelTo3dRay(tuple(pixel))
ray_z = [el / ray[2] for el in ray]
pt = [el * depth for el in ray_z]
point = PointStamped()
point.header.frame_id = frame
point.point.x = pt[0]
point.point.y = pt[1]
point.point.z = pt[2]
return point
def img_callback(self, msg):
cv_image = self.bridge.imgmsg_to_cv2(msg,
desired_encoding='passthrough')
#Respond to attribute error if subscribers haven't ran yet
try:
objects, img = self.yolo.search_for_objects(cv_image)
self.img_pub.publish(
self.bridge.cv2_to_imgmsg(img, encoding='passthrough'))
model = PinholeCameraModel()
model.fromCameraInfo(self.cam_info)
except AttributeError:
print("waiting")
return
if (len(objects) != 0):
for obj in objects:
print(obj)
xy = obj["Point"]
dist = self.depth_finder.get_depth(int(xy[0]), int(xy[1]))
depth = dist[0]
vect = model.projectPixelTo3dRay((xy[0], xy[1]))
xyz = [el / vect[2] for el in vect]
stampedPoint = PointStamped()
stampedPoint.header.frame_id = "head_rgbd_sensor_rgb_frame"
stampedPoint.point.x = xyz[0] * depth
stampedPoint.point.y = xyz[1] * depth
stampedPoint.point.z = depth
rospy.wait_for_service('transform_point')
get_3d_points = rospy.ServiceProxy('transform_point',
LocalizePoint)
resp = get_3d_points(stampedPoint)
#print(resp.localizedPointMap)
threeDPoint = resp.localizedPointMap
dictMsg = {}
dictMsg["name"] = obj["Label"]
dictMsg["type"] = "object"
dictMsg["coords"] = [
threeDPoint.point.x, threeDPoint.point.y,
threeDPoint.point.z
]
dictMsg["others"] = {}
self.nav_pub.publish(json.dumps(dictMsg))
else:
print("No objects found.")
def unit_vector_to_point(self, x, y):
'''
Creates a unit vector from the camera's frame, given a pixel point from the image
'''
height, width, depth = self.frame # Obtain the dimensions of the frame
cam_info = rospy.wait_for_message("/cameras/" + self.camera_viewer +
"/camera_info",
CameraInfo,
timeout=None)
img_proc = PinholeCameraModel()
img_proc.fromCameraInfo(cam_info)
# The origin of the camera is initally set to the top left corner
# However the projectPixelTo3dRay uses the origin at the center of the camera. Hence the coordinates have to be converted
x = x - width / 2
y = height / 2 - y
# Creates a unit vector to the given point. Unit vector passes through point from the center of the camera
n = img_proc.projectPixelTo3dRay((x, y))
return n
class ceilingLocalizer():
def __init__(self, image_topic, camera_topic, robot_depth, robot_frame, pattern):
# A subscriber to the image topic
rospy.Subscriber(image_topic, Image, self.image_callback)
# To get which frame is the camera frame
rospy.Subscriber(camera_topic, CameraInfo, self.camera_callback)
# To make the pixel to vector projection
self.camModel = PinholeCameraModel()
# How far is the robot plane from the camera?
self.robot_depth = robot_depth
# What is the name of the frame we should publish?
self.robot_frame = robot_frame
# Pattern to match
self.image_orig = cv2.imread(pattern, 1)
image_bin = cv2.cvtColor(self.image_orig, cv2.COLOR_BGR2GRAY)
ret, image_bin = cv2.threshold(image_bin, 127,255,cv2.THRESH_BINARY_INV)
contours, hierarchy = cv2.findContours(image_bin, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
sorted_contours = sorted(contours, key = cv2.contourArea, reverse = True)
# Our pattern is the second contour because for some reason its finding the edges of our image as a contour
self.pattern = sorted_contours[1]
self.pattern_area = 200
# To convert images from ROS to CV formats
self.bridge = CvBridge()
# Publish frames to tf
self.broadcaster = tf.TransformBroadcaster()
# Publisher for edited video
self.video = rospy.Publisher('ceilingMarker', Image)
def camera_callback(self, cameraInfo):
self.cameraInfo = cameraInfo
self.camModel.fromCameraInfo(cameraInfo)
self.cameraFrame = cameraInfo.header.frame_id
def image_callback(self, image_in):
image_cv = self.bridge.imgmsg_to_cv(image_in, 'bgr8')
image_cv2 = numpy.array(image_cv, dtype=numpy.uint8)
#image_cv2 = cv2.GaussianBlur(image_cv2,(5,5), 100, 100)
# Convert the image from BGR to HSV
image_hsv_cv2 = cv2.cvtColor(image_cv2, cv2.COLOR_BGR2HSV)
# Convert the image to grayscale
image_gray = cv2.cvtColor(image_cv2, cv2.COLOR_BGR2GRAY)
# Threshold grayscale image
flag, thresh = cv2.threshold(image_gray, 230, 255, cv2.THRESH_BINARY)
# Make a copy for contour detection
thresh_copy = numpy.copy(thresh)
contours, hierarchy = cv2.findContours(thresh_copy, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
# Sort contours by how much they match our pattern
potential_markers = self.find_potential_markers(contours)
# Ranges in hsv
lower_red_hsv = numpy.array([0,50,50])
upper_red_hsv = numpy.array([10,255,255])
lower_blue_hsv = numpy.array([105,50,50])
upper_blue_hsv = numpy.array([120,255,255])
lower_red_hsv2 = numpy.array([160,50,50])
upper_red_hsv2 = numpy.array([179,255,255])
# Make masks from our ranges
mask_red_hsv = numpy.bitwise_or(cv2.inRange(image_hsv_cv2, lower_red_hsv, upper_red_hsv), cv2.inRange(image_hsv_cv2, lower_red_hsv2, upper_red_hsv2))
mask_blue_hsv = cv2.inRange(image_hsv_cv2, lower_blue_hsv, upper_blue_hsv)
# Front should be red, rear should be blue
front_mask = mask_red_hsv
rear_mask = mask_blue_hsv
if len(potential_markers) >= 2:
# Set one marker as the front, the other the rear
if self.isFrontDominant(potential_markers[0], front_mask, rear_mask):
front_marker = potential_markers[0]
rear_marker = potential_markers[1]
else
front_marker = potential_markers[1]
rear_marker = potential_markers[0]
# Find the centerpoints
[frontx,fronty] = self.find_center(front_marker)
[backx,backy] = self.find_center(rear_marker)
# Find the midpoint
midx = (frontx+backx)/2
midy = (fronty+backy)/2
[vx,vy,vz] = self.camModel.projectPixelTo3dRay((midx,midy))
marker_depth = self.robot_depth / vz
# Position of the robot relative to ceiling kinect
x = marker_depth * vx
y = marker_depth * vy
z = self.robot_depth
# Orientation of robot
yaw = math.atan2((fronty-backy), (frontx-backx))
quaternion = tf.transformations.quaternion_from_euler(0.0,0.0,yaw, axes='sxyz')
# Broadcast the frame to tf
self.broadcaster.sendTransform((x,y,z), quaternion, rospy.Time.now(), self.robot_frame, self.cameraFrame)
self.draw_orientation(image_cv2, (frontx, fronty), (rearx, reary))
else:
rospy.loginfo("No marker found")
cv2.imshow('image', image_cv2)
cv2.imshow('image_gray', image_gray)
cv2.imshow('pattern', self.image_orig)
cv2.imshow('image_corners', image_corners)
cv2.imshow('thresh', thresh)
cv2.waitKey(3)
# Convert back to ROS Image msg
image_cv = cv.fromarray(image_cv2)
image_out = self.bridge.cv_to_imgmsg(image_cv, 'bgr8')
self.video.publish(image_out)
def isFrontDominant(self, contour, front_mask, rear_mask):
# Is the contour more red or more blue?
x,y,w,h = cv2.boundingRect(contour)
x1 = x - w/2
y1 = y - h/2
x2 = x + w/2
y2 = y + h/2
cropped_front = front_mask[y1:y2, x1:x2]
cropped_rear = rear_mask[y1:y2, x1:x2]
front_ratio = numpy.count_nonzero(cropped_front)/cropped_front.size
rear_ratio = numpy.count_nonzero(cropped_rear)/cropped_rear.size
if front_ratio >= rear_ratio:
return True
else
return False
def find_potential_markers(self, contours):
# Get rid of small contours and order by match strength
potential_markers = []
for contour in contours:
if cv2.contourArea(contour) > self.pattern_area:
potential_markers.append(contour)
sorted_markers = sorted(potential_markers, key=self.match_pattern)
return sorted_markers
def match_pattern(self, contour):
# Match the contour against our pattern
match = cv2.matchShapes(contour,self.pattern, 1, 0.0)
return match
def draw_orientation(self, image, front, back):
# Draw a line across the whole image for debug
slope = float(front[1] - back[1]) / float(front[0] - back[0])
leftmosty = int((-front[0]*slope) + front[1])
rightmosty = int(((image.shape[1]-front[0])*slope)+front[1])
cv2.line(image, (image.shape[1]-1, rightmosty), (0, leftmosty), (0,255,0), 3)
def find_center(self, contour):
moments = cv2.moments(contour, True)
x = int(moments['m10']/moments['m00'])
y = int(moments['m01']/moments['m00'])
return x,y
class ceilingLocalizer():
def __init__(self, image_topic, camera_topic, robot_depth):
# A subscriber to the image topic
rospy.Subscriber(image_topic, Image, self.image_callback)
# To get which frame is the camera frame
rospy.Subscriber(camera_topic, CameraInfo, self.camera_callback)
# To make the pixel to vector projection
self.camModel = PinholeCameraModel()
# How far is the robot plane from the camera?
self.robot_depth = robot_depth
# Alex's image stuff?
self.rcv = rcv()
# Publish frames to tf
self.broadcaster = tf.TransformBroadcaster()
def camera_callback(self, cameraInfo):
self.cameraInfo = cameraInfo
self.camModel.fromCameraInfo(cameraInfo)
def image_callback(self, image_in):
image_cv2 = self.rcv.toCv2(image_in)
#image_cv2 = cv2.GaussianBlur(image_cv2,(5,5), 100, 100)
# Convert the image from BGR to HSV
image_hsv_cv2 = cv2.cvtColor(image_cv2, cv2.COLOR_BGR2HSV)
# Ranges in rgb
lower_red_rgb = numpy.array([0,0,150])
upper_red_rgb = numpy.array([150,150,255])
lower_blue_rgb = numpy.array([150,0,0])
upper_blue_rgb = numpy.array([255,150,150])
# Ranges in hsv
lower_red_hsv = numpy.array([0,50,50])
upper_red_hsv = numpy.array([10,255,255])
lower_blue_hsv = numpy.array([105,50,50])
upper_blue_hsv = numpy.array([120,255,255])
lower_red_hsv2 = numpy.array([160,50,50])
upper_red_hsv2 = numpy.array([179,255,255])
# Make masks from our ranges
mask_red_rgb = cv2.inRange(image_cv2, lower_red_rgb, upper_red_rgb)
mask_red_hsv = numpy.bitwise_or(cv2.inRange(image_hsv_cv2, lower_red_hsv, upper_red_hsv), cv2.inRange(image_hsv_cv2, lower_red_hsv2, upper_red_hsv2))
mask_blue_rgb = cv2.inRange(image_cv2, lower_blue_rgb, upper_blue_rgb)
mask_blue_hsv = cv2.inRange(image_hsv_cv2, lower_blue_hsv, upper_blue_hsv)
# Combine masks
mask_red = numpy.bitwise_and(mask_red_rgb, mask_red_hsv)
mask_blue = numpy.bitwise_and(mask_blue_rgb, mask_blue_hsv)
# Copies of masks for imshow
mask_red_out = numpy.copy(mask_red)
mask_blue_out = numpy.copy(mask_blue)
# Find the largest contour in each mask
front_marker = self.find_marker(mask_red)
back_marker = self.find_marker(mask_blue)
# Find the centerpoints
[frontx,fronty] = self.find_center(front_marker)
[backx,backy] = self.find_center(back_marker)
# Find the midpoint
midx = (frontx+backx)/2
midy = (fronty+backy)/2
# Find the unit vector
distance = math.sqrt(math.pow(backx-frontx, 2) + math.pow(backy-fronty, 2))
uvx = (frontx-backx) / distance
uvy = (fronty-backy) / distance
cv2.circle(image_cv2,(midx,midy), 10, (0,255,0),cv2.cv.CV_FILLED,8,0)
# The adjacent side of our triangle (from kinect to floor minus turtlebot height)
adjacent = self.robot_depth
[vx,vy,vz] = self.camModel.projectPixelTo3dRay((midx,midy))
marker_depth = adjacent / vz
# Position of the robot relative to ceiling kinect
x = marker_depth * vx
y = marker_depth * vy
z = adjacent
# Orientation of robot
yaw = math.atan2((fronty-backy), (frontx-backx))
quaternion = tf.transformations.quaternion_from_euler(0.0,0.0,yaw, axes='sxyz')
ceiling_optical_frame = "ceiling_rgb_optical_frame"
# Broadcast the frame to tf
self.broadcaster.sendTransform((x,y,z), quaternion, rospy.Time.now(), "base_top", ceiling_optical_frame)
# Show all the images
cv2.imshow('mask_red', mask_red_out)
cv2.imshow('mask_blue', mask_blue_out)
cv2.imshow('image', image_cv2)
cv2.waitKey(3)
def find_marker(self, mask):
contours, hierarchy = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
largest = sorted(contours, key = cv2.contourArea,reverse = True)[:1]
return largest[0]
def find_center(self, contour):
moments = cv2.moments(contour, True)
x = int(moments['m10']/moments['m00'])
y = int(moments['m01']/moments['m00'])
return x,y
class Crane_Gripper(object):
def __init__(self,graspPlanner,cam,options,gripper):
#topic_name = '/hsrb/head_rgbd_sensor/depth_registered/image_raw'
not_read = True
while not_read:
try:
cam_info = cam.read_info_data()
if(not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
self.options = options
self.pcm.fromCameraInfo(cam_info)
self.br = tf.TransformBroadcaster()
self.tl = tf.TransformListener()
self.gp = graspPlanner
self.gripper = gripper
self.com = COM()
self.count = 0
def compute_trans_to_map(self,norm_pose,rot):
time.sleep(0.5)
pose = self.tl.lookupTransform('map','rgbd_sensor_rgb_frame_map', rospy.Time(0))
M = tf.transformations.quaternion_matrix(pose[1])
M_t = tf.transformations.translation_matrix(pose[0])
M[:,3] = M_t[:,3]
M_g = tf.transformations.quaternion_matrix(rot)
M_g_t = tf.transformations.translation_matrix(norm_pose)
M_g[:,3] = M_g_t[:,3]
M_T = np.matmul(M,M_g)
trans = tf.transformations.translation_from_matrix(M_T)
quat = tf.transformations.quaternion_from_matrix(M_T)
return trans,quat
def loop_broadcast(self,norm_pose,base_rot,rot_z):
norm_pose,rot = self.compute_trans_to_map(norm_pose,base_rot)
print "NORM POSE ",norm_pose
count = np.copy(self.count)
while True:
self.br.sendTransform((norm_pose[0], norm_pose[1], norm_pose[2]),
#tf.transformations.quaternion_from_euler(ai=0.0,aj=0.0,ak=0.0),
rot,
rospy.Time.now(),
'grasp_i_'+str(count),
#'head_rgbd_sensor_link')
'map')
self.br.sendTransform((0.0, 0.0, -cfg.GRIPPER_HEIGHT),
tf.transformations.quaternion_from_euler(ai=0.0,aj=0.0,ak=rot_z),
rospy.Time.now(),
'grasp_'+str(count),
#'head_rgbd_sensor_link')
'grasp_i_'+str(count))
def broadcast_poses(self,position,rot):
#while True:
count = 0
td_points = self.pcm.projectPixelTo3dRay((position[0],position[1]))
print "DE PROJECTED POINTS ",td_points
norm_pose = np.array(td_points)
norm_pose = norm_pose/norm_pose[2]
norm_pose = norm_pose*(cfg.MM_TO_M*position[2])
print "NORMALIZED POINTS ",norm_pose
#pose = np.array([td_points[0],td_points[1],0.001*num_pose[2]])
a = tf.transformations.quaternion_from_euler(ai=-2.355,aj=-3.14,ak=0.0)
b = tf.transformations.quaternion_from_euler(ai=0.0,aj=0.0,ak=1.57)
base_rot = tf.transformations.quaternion_multiply(a,b)
thread.start_new_thread(self.loop_broadcast,(norm_pose,base_rot,rot))
time.sleep(0.8)
def convert_crop(self,pose):
pose[0] = self.options.OFFSET_Y + pose[0]
pose[1] = self.options.OFFSET_X + pose[1]
return pose
def plot_on_true(self,pose,true_img):
#pose = self.convert_crop(pose)
# dp = DrawPrediction()
# image = dp.draw_prediction(np.copy(true_img),pose)
# cv2.imshow('label_given',image)
# cv2.waitKey(30)
pass
def get_grasp_pose(self,x,y,z,rot,c_img=None):
'''
Evaluates the current policy and then executes the motion
specified in the the common class
'''
poses = []
#IPython.embed()
p_list = []
pose = [x,y,z]
self.plot_on_true([x,y],c_img)
self.broadcast_poses(pose,rot)
grasp_name = 'grasp_'+str(self.count)
self.count += 1
return grasp_name
def open_gripper(self):
try:
self.gripper.command(1.2)
except:
rospy.logerr('grasp open error')
def close_gripper(self):
try:
self.gripper.grasp(-0.1)
except:
rospy.logerr('grasp close error')
def half_gripper(self):
try:
self.gripper.command(0.6)
except:
rospy.logerr('grasp close error')
class PeopleDetection:
""" People Detection class with useful functions for getting coordinates of detections"""
def __init__(self):
self._net = YOLOv5(model_path, device) #jetson.inference.detectNet("ssd-mobilenet-v2")
self.img = None
self.width = None
self.height = None
self.need_cam_info = True
self.camera_model = PinholeCameraModel()
self.marker = Marker()
self.prev_time = 0
self.marker_pub = rospy.Publisher("visualization_markers", Marker, queue_size=10)
self.camera_info = rospy.Subscriber('/camera/depth/camera_info', CameraInfo, self.info_callback)
def get_detections(self, image):
"""
Function that uses a SSD Mobilenet V2 model to run an inference on provided RGBA image at variable FPS
:param image: RGBA image frame from realsense camera
:return: List of detections found on the provided image and
resulting image with bounding boxes, labels, and confidence %
"""
self.prev_time = time()
self.img = image
self.width = image.shape[1]
self.height = image.shape[0]
detections = self._net.predict(self.img)
# if detections.n > 0:
# for *xyxy, cond, cls in detections.pred[0]:
# if cls == 0:
# label = f'{detections.names[int(cls)]} {cond:.2f}'
# plot_one_box(xyxy, self.img, label=label, color=colors[int(cls)%10], line_thickness=3)
print("FPS: {}".format(1/(time() - self.prev_time)))
return detections, self.img
def get_person_coordinates(self, depth_image, detections):
"""
Function that filters through detections and calculates the 3d coordinates of every person detected in list of
detections
:param depth_image: grayscale depth frame from realsense camera
:param detections: list of detections found from rgb inference
:return: list of coordinates of every person's coordinates
"""
coord_list = []
for *xywh, cond, cls in detections.xywh[0]:
if cls == 0 and cond > 0.7:
x,y,w,h = int(xywh[0]),int(xywh[1]),int(xywh[2]),int(xywh[3]),
depth = depth_image[int(y), int(x)]
depth_array = depth_image[int(y-h/5):int(y+h/10),int(x-w/4):int(x+w/4)].flatten()
depth = np.median(depth_array[np.nonzero(depth_array)]) / 1000
person_coord = self._get_coord(depth, x, y)
#print(f'{x} {y} {depth:.2f}')
self.marker = self.make_marker(person_coord)
coord_list.append(person_coord)
self.marker_pub.publish(self.marker)
return coord_list
def _get_coord(self, person_depth, x, y):
"""
Helper function to calculate 3d coordinates using image_geometry package given pixel of person detected and
respective depth value mapping
:param person_depth: depth value at pixel representing center of person detected
:param x: horizontal pixel value
:param y: vertial pixel value
:return: list of [x,y,z] of person relative to camera
"""
unit_vector = self.camera_model.projectPixelTo3dRay((x, y))
normalized_vector = [i / unit_vector[2] for i in unit_vector]
point_3d = [j * person_depth for j in normalized_vector]
return point_3d
def make_marker(self, point_3d):
"""
Function that creates Marker Spheres for people detected for visualization of people with respect to the camera
and car (given camera is attached to car)
Adds detections to a MarkerArray List
:param point_3d: calcualted 3d point of person in image
:param count: number of people detected
"""
person_marker = Marker()
person_marker.header.frame_id = "map"
person_marker.ns = "person"
person_marker.type = person_marker.SPHERE
person_marker.action = person_marker.ADD
person_marker.id = 0
person_marker.pose.position.x = point_3d[0]
person_marker.pose.position.y = point_3d[2]
person_marker.pose.position.z = point_3d[1]
person_marker.pose.orientation.x = 0.0
person_marker.pose.orientation.y = 0.0
person_marker.pose.orientation.z = 0.0
person_marker.pose.orientation.w = 1.0
person_marker.scale.x = 1
person_marker.scale.y = 1
person_marker.scale.z = 1
person_marker.color.a = 1.0
person_marker.color.r = 0.0
person_marker.color.g = 1.0
person_marker.color.b = 0.0
person_marker.lifetime = rospy.Duration(1)
return person_marker
def info_callback(self, info):
""" Helper callback function for getting camera info for image_geometry package, only used one time"""
if self.need_cam_info:
print("got camera info")
self.camera_model.fromCameraInfo(info)
self.need_cam_info = False
def calibrate(self, image, info, depth):
model = PinholeCameraModel()
model.fromCameraInfo(info)
try:
cv_img = self.bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print (e)
return False
try:
cv_depth = self.bridge.imgmsg_to_cv2(depth)
except CvBridgeError as e:
print (e)
return False
cv_depth = cv2.medianBlur(cv_depth, 5)
cv_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(
cv_img,
(9, 6),
None,
flags=cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_FILTER_QUADS | cv2.CALIB_CB_NORMALIZE_IMAGE,
)
if not ret:
rospy.logerr("Could not find chessboard corners")
return False
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
cv2.cornerSubPix(cv_img, corners, (11, 11), (-1, -1), criteria)
corners = corners.reshape(1, -1, 2)[0]
points = []
ppoints = []
ppp = PoseArray()
ppp.header.stamp = rospy.Time.now()
ppp.header.frame_id = self.world_frame
for c in corners:
pt = list(model.projectPixelTo3dRay((c[0], c[1])))
pt[:] = [x / pt[2] for x in pt]
# depth image is noisy - let's make mean of few pixels
da = []
for x in range(int(c[0]) - 2, int(c[0]) + 3):
for y in range(int(c[1]) - 2, int(c[1]) + 3):
da.append(cv_depth[y, x] / 1000.0)
d = np.mean(da)
pt[:] = [x * d for x in pt]
ps = PointStamped()
ps.header.stamp = rospy.Time(0)
ps.header.frame_id = image.header.frame_id
ps.point.x = pt[0]
ps.point.y = pt[1]
ps.point.z = pt[2]
# transform 3D point from camera into the world coordinates
try:
ps = self.tfl.transformPoint(self.world_frame, ps)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logerr("can't get transform")
return False
pp = Pose()
pp.position.x = ps.point.x
pp.position.y = ps.point.y
pp.position.z = ps.point.z
pp.orientation.x = 0
pp.orientation.y = 0
pp.orientation.z = 0
pp.orientation.w = 1.0
ppp.poses.append(pp)
# store x,y -> here we assume that points are 2D (on tabletop)
points.append([self.rpm * ps.point.x, self.rpm * ps.point.y])
self.corners_pub.publish(ppp)
dx = (self.pix_label.width() - self.pix_label.pixmap().width()) / 2.0
dy = (self.pix_label.height() - self.pix_label.pixmap().height()) / 2.0
box_size = self.pix_label.pixmap().width() / 12.0
# TODO self.scene_origin ???
# generate requested table coordinates
for y in range(0, 6):
for x in range(0, 9):
px = 2 * box_size + x * box_size + dx
py = 2 * box_size + y * box_size + dy
ppoints.append([px, py])
h, status = cv2.findHomography(np.array(points), np.array(ppoints), cv2.LMEDS)
self.h_matrix = np.matrix(h)
# self.h_matrix = np.matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1.0]])
# store homography matrix to parameter server
s = str(self.h_matrix.tolist())
rospy.set_param("/art/interface/projected_gui/projector/" + self.proj_id + "/calibration_matrix", s)
print s
return True
def calibrate(self, image, info, depth):
model = PinholeCameraModel()
model.fromCameraInfo(info)
try:
cv_img = self.bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print(e)
return False
try:
cv_depth = self.bridge.imgmsg_to_cv2(depth)
except CvBridgeError as e:
print(e)
return False
cv_depth = cv2.medianBlur(cv_depth, 5)
cv_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(
cv_img, (9, 6),
None,
flags=cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_FILTER_QUADS
| cv2.CALIB_CB_NORMALIZE_IMAGE)
if not ret:
rospy.logerr("Could not find chessboard corners")
return False
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100,
0.001)
cv2.cornerSubPix(cv_img, corners, (11, 11), (-1, -1), criteria)
corners = corners.reshape(1, -1, 2)[0]
points = []
ppoints = []
ppp = PoseArray()
ppp.header.stamp = rospy.Time.now()
ppp.header.frame_id = self.world_frame
for c in corners:
pt = list(model.projectPixelTo3dRay((c[0], c[1])))
pt[:] = [x / pt[2] for x in pt]
# depth image is noisy - let's make mean of few pixels
da = []
for x in range(int(c[0]) - 2, int(c[0]) + 3):
for y in range(int(c[1]) - 2, int(c[1]) + 3):
da.append(cv_depth[y, x] / 1000.0)
d = np.mean(da)
pt[:] = [x * d for x in pt]
ps = PointStamped()
ps.header.stamp = rospy.Time(0)
ps.header.frame_id = image.header.frame_id
ps.point.x = pt[0]
ps.point.y = pt[1]
ps.point.z = pt[2]
# transform 3D point from camera into the world coordinates
try:
ps = self.tfl.transformPoint(self.world_frame, ps)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logerr("Can't get transform")
return False
pp = Pose()
pp.position.x = ps.point.x
pp.position.y = ps.point.y
pp.position.z = ps.point.z
pp.orientation.x = 0
pp.orientation.y = 0
pp.orientation.z = 0
pp.orientation.w = 1.0
ppp.poses.append(pp)
# store x,y -> here we assume that points are 2D (on tabletop)
points.append([self.rpm * ps.point.x, self.rpm * ps.point.y])
self.corners_pub.publish(ppp)
dx = (self.pix_label.width() - self.pix_label.pixmap().width()) / 2.0
dy = (self.pix_label.height() - self.pix_label.pixmap().height()) / 2.0
box_size = self.pix_label.pixmap().width() / 12.0
# TODO self.scene_origin ???
# generate requested table coordinates
for y in range(0, 6):
for x in range(0, 9):
px = 2 * box_size + x * box_size + dx
py = 2 * box_size + y * box_size + dy
ppoints.append([px, py])
h, status = cv2.findHomography(np.array(points), np.array(ppoints),
cv2.LMEDS)
h_matrix = np.matrix(h)
self.emit(QtCore.SIGNAL('show_pix_label'), False) # hide chessboard
self.calibrating = False
self.calibrated = True
self.calibrated_pub.publish(self.is_calibrated())
# store homography matrix to parameter server
s = str(h_matrix.tolist())
rospy.set_param("~calibration_matrix", s)
self.init_map_from_matrix(h_matrix)
# self.h_matrix = np.matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1.0]])
return True
print('==========Initializing Subscribers=====')
cam_info=rospy.wait_for_message("/camera/rgb/camera_info", CameraInfo, timeout=None)
sub_rgb = rospy.Subscriber("/camera/rgb/image_raw", Image, callback_rgb)
sub_depth = rospy.Subscriber("/camera/depth_registered/hw_registered/image_rect", Image, callback_depth)
sleep(2)
setup_feed()
imgproc=PC()
imgproc.fromCameraInfo(cam_info)
# print cam_info.P
# print (np.dstack((depth_mask,depth_mask,depth_mask))).shape
rate = rospy.Rate(20)
while not rospy.is_shutdown():
# cv2.imshow(win1_name, np.hstack([rgb_image,depth_image]))
cv2.imshow(win1_name,rgb_image)
key = cv2.waitKey(1) & 0xFF
if key == 27: # wait for ESC key to exit
rospy.signal_shutdown("User hit ESC key to quit.")
obj_coord= np.array(imgproc.projectPixelTo3dRay((circle_coord[0],circle_coord[1])))
# scale=obj_depth/obj_coord[2] #scale the unit vector according to measured depth
# obj_coord=obj_coord*scale
# print obj_coord
rate.sleep()
rospy.spin()
def callback_pos(self, msg):
if self.detector_switch == 'on':
x_c = 0.0
y_c = 0.0
z_c = 0.0
x_org = 0.0
y_org = 0.0
z_org = 0.0
if self.img_detection_fnc:
# original drone location:
x_org = msg.position.x
y_org = msg.position.y
z_org = msg.position.z
self.current_x = x_org
self.current_y = y_org
self.current_z = z_org
# need to transform back to the Eular angle
orientation_list = [
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
]
self.current_yaw = round(
self.RAD_to_DEG *
euler_from_quaternion(orientation_list)[2])
# camera location relative to drone:
x_c = 0.0
y_c = 0.0
z_c = 0.0
else:
pass
if self.pixel_center != []:
pose = Pose()
point_msg = PoseStamped()
# create vectors to scale the normalized coordinates in camera frame
t_1 = np.array([[x_org, y_org, z_org]])
t_2 = np.array([[x_c, y_c, z_c]])
# translation matrix from world to camera
t_W2C = np.add(t_1, t_2)
# transformation from image coordinate to camera coordinate
cam_info = rospy.wait_for_message(
"/firefly/vi_sensor/camera_depth/camera/camera_info",
CameraInfo,
timeout=None)
img_proc = PinholeCameraModel()
img_proc.fromCameraInfo(cam_info)
cam_model_point = img_proc.projectPixelTo3dRay(
self.pixel_center)
cam_model_point = np.round(np.multiply(
np.divide(cam_model_point, cam_model_point[2]), t_W2C[0,
2]),
decimals=0)
# creating a place holder to save the location info
point_msg.pose.position.x = cam_model_point[0]
point_msg.pose.position.y = cam_model_point[1]
point_msg.pose.position.z = cam_model_point[2]
point_msg.pose.orientation.x = 0
point_msg.pose.orientation.y = 0
point_msg.pose.orientation.z = 0
point_msg.pose.orientation.w = 1
point_msg.header.stamp = rospy.Time.now()
point_msg.header.frame_id = img_proc.tfFrame()
# initiate the coordinate (frame) transformation
self.tf_listener.waitForTransform(img_proc.tfFrame(), "world",
rospy.Time.now(),
rospy.Duration(1.0))
# calculate the transformed coordinates
tf_point = self.tf_listener.transformPose("world", point_msg)
x_new = 0.0 # round(tf_point.pose.position.x, 2)
y_new = 0.0 # round(tf_point.pose.position.y, 2)
z_new = msg.position.z - 0.01
# z_new = (1 - 0.01 * self.decent_rate) * msg.position.z
pose.position.x = x_new
pose.position.y = y_new
pose.position.z = z_new
pose.orientation.x = msg.orientation.x
pose.orientation.y = msg.orientation.y
pose.orientation.z = msg.orientation.z
pose.orientation.w = msg.orientation.w
self.pos_pub.publish(pose)
else:
# If there is no landing mark detected
pose = Pose()
# original drone location:
x_org = msg.position.x
y_org = msg.position.y
z_org = msg.position.z
self.current_x = x_org
self.current_y = y_org
self.current_z = z_org
# need to transform back to the Eular angle
orientation_list = [
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
]
self.current_yaw = round(
self.RAD_to_DEG *
euler_from_quaternion(orientation_list)[2])
pose.position.x = msg.position.x
pose.position.y = msg.position.y
pose.position.z = msg.position.z
pose.orientation.x = msg.orientation.x
pose.orientation.y = msg.orientation.y
pose.orientation.z = msg.orientation.z
pose.orientation.w = msg.orientation.w
self.pos_pub.publish(pose)
else:
# if the detection switch doesn't turn on, skip the coordinate computation
pass
class PeopleDetection:
""" People Detection class with useful functions for getting coordinates of detections"""
def __init__(self):
self._net = jetson.inference.detectNet("ssd-mobilenet-v2")
self.img = None
self.width = None
self.height = None
self.need_cam_info = True
self.camera_model = PinholeCameraModel()
self.marker_array = MarkerArray()
self.marker_pub = rospy.Publisher("visualization_markers",
MarkerArray,
queue_size=500)
self.camera_info = rospy.Subscriber('/camera/depth/camera_info',
CameraInfo, self.info_callback)
def get_detections(self, image):
"""
Function that uses a SSD Mobilenet V2 model to run an inference on provided RGBA image at variable FPS
:param image: RGBA image frame from realsense camera
:return: List of detections found on the provided image and
resulting image with bounding boxes, labels, and confidence %
"""
self.img = jetson.utils.cudaFromNumpy(image)
self.width = image.shape[1]
self.height = image.shape[0]
detections = self._net.Detect(self.img, self.width, self.height)
print("The inference is happening at " +
str(self._net.GetNetworkFPS()) + " FPS")
return detections, jetson.utils.cudaToNumpy(self.img)
def get_person_coordinates(self, depth_image, detections):
"""
Function that filters through detections and calculates the 3d coordinates of every person detected in list of
detections
:param depth_image: grayscale depth frame from realsense camera
:param detections: list of detections found from rgb inference
:return: list of coordinates of every person's coordinates
"""
coord_list = []
count = 0
for det in detections:
count = count + 1
if det.ClassID == 1:
person_center = det.Center
x, y = person_center
depth_arr = []
try:
for x in range(int(x) - 2, int(x) + 3):
for y in range(int(y) - 2, int(y) + 3):
depth_arr.append(
depth_image[int(x), int(y)] / 1000.0)
depth = np.mean(depth_arr)
person_coord = self._get_coord(depth, x, y)
self.make_marker(person_coord, count)
coord_list.append(person_coord)
except IndexError:
self.marker_pub.publish(self.marker_array)
self.marker_pub.publish(self.marker_array)
return coord_list
def _get_coord(self, person_depth, x, y):
"""
Helper function to calculate 3d coordinates using image_geometry package given pixel of person detected and
respective depth value mapping
:param person_depth: depth value at pixel representing center of person detected
:param x: horizontal pixel value
:param y: vertial pixel value
:return: list of [x,y,z] of person relative to camera
"""
unit_vector = self.camera_model.projectPixelTo3dRay((x, y))
normalized_vector = [i / unit_vector[2] for i in unit_vector]
point_3d = [j * person_depth for j in normalized_vector]
return point_3d
def make_marker(self, point_3d, count):
"""
Function that creates Marker Spheres for people detected for visualization of people with respect to the camera
and car (given camera is attached to car)
Adds detections to a MarkerArray List
:param point_3d: calcualted 3d point of person in image
:param count: number of people detected
"""
person_marker = Marker()
person_marker.header.frame_id = "map"
person_marker.ns = "person"
person_marker.type = person_marker.SPHERE
person_marker.action = person_marker.ADD
person_marker.id = count
person_marker.pose.position.x = point_3d[0] * -1
person_marker.pose.position.y = point_3d[2]
person_marker.pose.position.z = point_3d[1]
person_marker.pose.orientation.x = 0.0
person_marker.pose.orientation.y = 0.0
person_marker.pose.orientation.z = 0.0
person_marker.pose.orientation.w = 1.0
person_marker.scale.x = 0.25
person_marker.scale.y = 0.25
person_marker.scale.z = 0.25
person_marker.color.a = 1.0
person_marker.color.r = 0.0
person_marker.color.g = 1.0
person_marker.color.b = 0.0
person_marker.lifetime = rospy.Duration(1)
self.marker_array.markers.append(person_marker)
def info_callback(self, info):
""" Helper callback function for getting camera info for image_geometry package, only used one time"""
if self.need_cam_info:
print("got camera info")
self.camera_model.fromCameraInfo(info)
self.need_cam_info = False
as desired. """
scale = z_desired / xyz[2]
xyz = tuple(el * scale for el in xyz)
return xyz
if __name__ == "__main__":
bag_path = sys.argv[1] # path to camera_info
bag = rosbag.Bag(bag_path)
camera_infos = bag.read_messages(topics='/camera/rgb/camera_info')
camera_info = camera_infos.next()[1]
model = PinholeCameraModel()
model.fromCameraInfo(camera_info)
# Fig. 1: How many meters per pixel at various depths?
print model.projectPixelTo3dRay((319.5, 239.5))
u_neutral = 319.5
v_neutral = 239.5
depths = [d * 0.10 for d in range(10,50)] # approximate cutoff distance: 4m
res = []
for depth in depths:
primus = model.projectPixelTo3dRay((u_neutral, v_neutral))
secundus = model.projectPixelTo3dRay((u_neutral+1, v_neutral))
primus_at_depth = project_onto_depth(primus, depth)
secundus_at_depth = project_onto_depth(secundus, depth)
dXYZ = tuple(s - p for p,s in zip(primus_at_depth, secundus_at_depth))
res.append([dXYZ[0], 1.39 / dXYZ[0]])
print res
class ROSTensorFlow(object):
def __init__(self):
# Setup tensorflow (v1.14 / v2.0 compatible)
#tf.compat.v1.disable_eager_execution()
#np.set_printoptions(threshold=sys.maxsize)
# Init CV bridge
self.cv_bridge1 = CvBridge()
# Setup raytracing and transform
cam_info = rospy.wait_for_message("/door_kinect/rgb/camera_info",
CameraInfo,
timeout=None)
self.img_proc = PinholeCameraModel()
self.img_proc.fromCameraInfo(cam_info)
self.tf_broadcaster = tf.TransformBroadcaster()
self.camera_frame = 'door_kinect_rgb_optical_frame'
# Subscribe to RGB and D data topics
self.sub_rgb = message_filters.Subscriber(
"/door_kinect/rgb/image_color", Image)
self.sub_d = message_filters.Subscriber(
"/door_kinect/depth_registered/sw_registered/image_rect", Image)
# Setup publishing topics
self.pub_rgb = rospy.Publisher("/humandetect_imagergb",
Image,
queue_size=1)
self.pub_depth = rospy.Publisher("/humandetect_imagedepth",
Image,
queue_size=1)
self.pub_pose = rospy.Publisher("/humandetect_poses",
PoseArray,
queue_size=1)
# Synchronisation
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.sub_rgb, self.sub_d], 10, 0.1, allow_headerless=False)
# Setup Tensorflow object detection
# Tensorflow path setup
path_add = os.path.join(
rospkg.RosPack().get_path("yeetbot_humantracker"),
"models/research")
sys.path.append(path_add)
os.path.join(path_add, "slim")
sys.path.append(path_add)
objectdetect_path = os.path.join(
rospkg.RosPack().get_path("yeetbot_humantracker"),
"models/research/object_detection")
sys.path.append(objectdetect_path)
print(sys.path)
# Import object detection API utils
from utils import label_map_util
from utils import visualization_utils as viz_utils
self.label_map_util1 = label_map_util
self.viz_utils1 = viz_utils
# Tensorflow model setup
MODEL_NAME = 'ssdlite_mobilenet_v2_coco_2018_05_09'
CWD_PATH = os.getcwd()
PATH_TO_CKPT = os.path.join(objectdetect_path, MODEL_NAME,
'frozen_inference_graph.pb')
PATH_TO_LABELS = os.path.join(objectdetect_path, 'data',
'mscoco_label_map.pbtxt')
NUM_CLASSES = 90
self.label_map = label_map_util.load_labelmap(PATH_TO_LABELS)
self.categories = label_map_util.convert_label_map_to_categories(
self.label_map, max_num_classes=NUM_CLASSES, use_display_name=True)
self.category_index = label_map_util.create_category_index(
self.categories)
print(self.category_index)
# Tensorflow session setup
self.sess = None
detection_graph = tensorflow.Graph()
with detection_graph.as_default():
od_graph_def = tensorflow.GraphDef()
with tensorflow.gfile.GFile(PATH_TO_CKPT, 'rb') as fid:
serialized_graph = fid.read()
od_graph_def.ParseFromString(serialized_graph)
tensorflow.import_graph_def(od_graph_def, name='')
self.sess = tensorflow.Session(graph=detection_graph)
# Tensorflow detection output setup
self.image_tensor = detection_graph.get_tensor_by_name(
'image_tensor:0')
self.detection_boxes = detection_graph.get_tensor_by_name(
'detection_boxes:0')
self.detection_scores = detection_graph.get_tensor_by_name(
'detection_scores:0')
self.detection_classes = detection_graph.get_tensor_by_name(
'detection_classes:0')
self.num_detections = detection_graph.get_tensor_by_name(
'num_detections:0')
self.frame_rate_calc = 1
self.freq = cv2.getTickFrequency()
self.font = cv2.FONT_HERSHEY_SIMPLEX
## Setup KCF tracker
#self.kcf_tracker = cv2.TrackerKCF_create()
self.ct = CentroidTracker()
# Callback register
self.ts.registerCallback(self.callback)
print('Init done')
def callback(self, color_msg, depth_msg):
t1 = cv2.getTickCount()
print(t1)
# Receive camera data
frame_color = self.cv_bridge1.imgmsg_to_cv2(
color_msg, desired_encoding="passthrough").copy()
frame_depth = self.cv_bridge1.imgmsg_to_cv2(
depth_msg, desired_encoding="passthrough").copy()
# Convert to format
frame_rgb = cv2.cvtColor(frame_color, cv2.COLOR_BGR2RGB)
frame_expanded = np.expand_dims(frame_rgb, axis=0)
# Get image geometry
rgb_height, rgb_width, rgb_channels = frame_rgb.shape
depth_height, depth_width = frame_depth.shape
print("RGB INFO")
print("{0} {1} {2}".format(rgb_height, rgb_width, rgb_channels))
print("DEPTH INFO")
print("{0} {1}".format(depth_height, depth_width))
# Find people on RGB image with Tensorflow
with self.sess.graph.as_default():
(boxes, scores, classes, num) = self.sess.run(
[
self.detection_boxes, self.detection_scores,
self.detection_classes, self.num_detections
],
feed_dict={self.image_tensor: frame_expanded})
#print("CLASSES")
#print(classes)
#print("NUM")
#print(int(num[0]))
#print("BOXES")
# Find people on image from Tensorflow results
boxes_coords = list()
center_points = list()
center_pointsd = list()
depth_coords = list()
depth_frames = list()
depths = list()
for i in range(int(num[0])):
if (int(classes[0][i]) == 1
and scores[0][i] >= 0.70): # if human detected
box = [None] * 2
box_depth = [None] * 2
min_y = int(boxes[0][i][0] * rgb_height)
min_x = int(boxes[0][i][1] * rgb_width)
max_y = int(boxes[0][i][2] * rgb_height)
max_x = int(boxes[0][i][3] * rgb_width)
# ok = tracker.init(frame, min_x, min_y, max_x, max_y)
box[0] = (min_x, min_y)
box[1] = (max_x, max_y)
center_x = int((min_x + max_x) / 2)
center_y = int((min_y + max_y) / 2)
center = (center_x, center_y)
box_w = max_x - min_x
box_h = max_y - min_y
depth_min_y = int(center_y - box_h / 4)
depth_max_y = int(center_y + box_h / 4)
depth_min_x = int(center_x - box_w / 4)
depth_max_x = int(center_x + box_w / 4)
box_depth[0] = (depth_min_x, depth_min_y)
box_depth[1] = (depth_max_x, depth_max_y)
boxes_coords.append(box)
depth_coords.append(box_depth)
center_points.append(center)
depth_frame = frame_depth[depth_min_y:depth_max_y,
depth_min_x:depth_max_x]
#np.nan_to_num(depth_frame, False, 10)
depth_frame = depth_frame
depth_median = np.nanmedian(depth_frame)
#print("TYPE")
#print(type(depth_frame))
#print(depth_frame)
depth_frames.append(depth_frame)
depths.append(depth_median)
center_d = (center_x, center_y, depth_median)
center_pointsd.append(center_d)
#print("OBJECT {0} HAS DEPTH {1}".format(i, depth_median))
#print(depth_frame[0])
#print("BOXES COORDS")
#print(boxes_coords)
#self.viz_utils1.visualize_boxes_and_labels_on_image_array(
# frame_color,
# np.squeeze(boxes),
# np.squeeze(classes).astype(np.int32),
# np.squeeze(scores),
# self.category_index,
# use_normalized_coordinates=True,
# line_thickness=8,
# min_score_thresh=0.70)
#cv2.imshow('Object detector', frame_color)
t2 = cv2.getTickCount()
time1 = (t2 - t1) / self.freq
frame_rate_calc = 1 / time1
print("FPS: {0:.2f}".format(frame_rate_calc))
objects = self.ct.update(center_pointsd.copy())
for (objectID, centroid) in objects.items():
text = "ID {}".format(objectID)
centroidx, centroidy, centroidz = centroid
centroidxy = (centroidx, centroidy)
cv2.putText(frame_color, text, (centroidx - 10, centroidy - 10),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
cv2.circle(frame_color, (centroidx, centroidy), 4, (0, 255, 0), -1)
#print(boxes_coords)
#print(depth_coords)
for box in boxes_coords:
cv2.rectangle(frame_color, box[0], box[1], (255, 0, 0), 2)
for depth_box in depth_coords:
cv2.rectangle(frame_color, depth_box[0], depth_box[1], (0, 255, 0),
2)
for point in center_points:
print(point)
cv2.circle(frame_color, point, 5, (0, 0, 255), 2)
#boxes_coords = list()
#center_points = list()
#depth_coords = list()
#depth_frames = list()
#depths = list()
out_pose = PoseArray()
out_pose.header.stamp = color_msg.header.stamp
out_pose.header.frame_id = self.camera_frame
poses1 = list()
for (objectID, centroid) in objects.items():
centroidx, centroidy, centroidz = centroid
print("OBJECT {0} AT {1} DEPTH {2}".format(objectID,
(centroidx, centroidy),
centroidz))
x, y, _ = self.img_proc.projectPixelTo3dRay((centroidx, centroidy))
print(x)
print(y)
z = (centroidz)
print(z)
self.tf_broadcaster.sendTransform(
[x, y, z], tf.transformations.quaternion_from_euler(0, 0, 0),
color_msg.header.stamp, '/humanpos_{0}'.format(i),
self.camera_frame)
quat = tf.transformations.quaternion_from_euler(0, 1.5, 0)
pose1 = Pose()
pose1.position.x = x
pose1.position.y = y
pose1.position.z = z
pose1.orientation.x = quat[0]
pose1.orientation.y = quat[1]
pose1.orientation.z = quat[2]
pose1.orientation.w = quat[3]
poses1.append(pose1)
out_pose.poses = poses1
self.pub_pose.publish(out_pose)
out_message = self.cv_bridge1.cv2_to_imgmsg(frame_color)
#print("TYPE2")
#print(type(depth_frame[0]))
#print("TYPE3")
#print(type(frame_color))
#out_message2 = self.cv_bridge1.cv2_to_imgmsg(depth_frames[0])
out_message.header.stamp = color_msg.header.stamp
#out_message2.header.stamp = depth_msg.header.stamp
try:
self.pub_rgb.publish(out_message)
#self.pub_depth.publish(out_message2)
except rospy.ROSException as e:
raise e
class PixelConversion():
def __init__(self):
self.image_topic = "wall_camera/image_raw"
self.image_info_topic = "wall_camera/camera_info"
self.point_topic = "/clicked_point"
self.frame_id = "/map"
self.pixel_x = 350
self.pixel_y = 215
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
# Create the OpenCV display window for the RGB image
self.cv_window_name = self.image_topic
cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
cv.MoveWindow(self.cv_window_name, 25, 75)
# Create the cv_bridge object
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber(self.image_topic, Image, self.image_cb, queue_size = 1)
self.image_info_sub = rospy.Subscriber(self.image_info_topic, CameraInfo, self.image_info_cb, queue_size = 1)
self.point_sub = rospy.Subscriber(self.point_topic, PointStamped, self.point_cb, queue_size = 4)
self.line_pub = rospy.Publisher("line", Marker, queue_size = 10)
self.inter_pub = rospy.Publisher("inter", Marker, queue_size = 10)
self.plane_pub = rospy.Publisher("plane", Marker, queue_size = 10)
self.points = []
self.ready_for_image = False
self.has_pic = False
self.camera_info_msg = None
self.tf_listener = tf.TransformListener()
# self.display_picture()
self.camera_model = PinholeCameraModel()
def point_cb(self, point_msg):
if len(self.points) < 4:
self.points.append(point_msg.point)
if len(self.points) == 1:
self.ready_for_image = True
print "Point stored from click: ", point_msg.point
def image_cb(self, img_msg):
if self.ready_for_image:
# Use cv_bridge() to convert the ROS image to OpenCV format
try:
frame = self.bridge.imgmsg_to_cv2(img_msg, "bgr8")
except CvBridgeError, e:
print e
# Convert the image to a Numpy array since most cv2 functions
# require Numpy arrays.
frame = np.array(frame, dtype=np.uint8)
print "Got array"
# Process the frame using the process_image() function
#display_image = self.process_image(frame)
# Display the image.
self.img = frame
# self.has_pic = True
if self.camera_info_msg is not None:
print "cx ", self.camera_model.cx()
print "cy ", self.camera_model.cy()
print "fx ", self.camera_model.fx()
print "fy ", self.camera_model.fy()
# Get the point from the clicked message and transform it into the camera frame
self.tf_listener.waitForTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0), rospy.Duration(0.5))
(trans,rot) = self.tf_listener.lookupTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0))
self.tfros = tf.TransformerROS()
tf_mat = self.tfros.fromTranslationRotation(trans, rot)
point_in_camera_frame = tuple(np.dot(tf_mat, np.array([self.points[0].x, self.points[0].y, self.points[0].z, 1.0])))[:3]
# Get the pixels related to the clicked point (the point must be in the camera frame)
pixel_coords = self.camera_model.project3dToPixel(point_in_camera_frame)
print "Pixel coords ", pixel_coords
# Get the unit vector related to the pixel coords
unit_vector = self.camera_model.projectPixelTo3dRay((int(pixel_coords[0]), int(pixel_coords[1])))
print "Unit vector ", unit_vector
# TF the unit vector of the pixel to the frame of the clicked point from the map frame
# self.tf_listener.waitForTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0), rospy.Duration(0.5))
# (trans,rot) = self.tf_listener.lookupTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0))
# self.tfros = tf.TransformerROS()
# tf_mat = self.tfros.fromTranslationRotation(trans, rot)
# xyz = tuple(np.dot(tf_mat, np.array([unit_vector[0], unit_vector[1], unit_vector[2], 1.0])))[:3]
# print "World xyz ", xyz
# intersect the unit vector with the ground plane
red = (0,125,255)
cv2.circle(self.img, (int(pixel_coords[0]), int(pixel_coords[1])), 30, red)
cv2.imshow(self.image_topic, self.img)
''' trying better calculations for going from pixel to world '''
# need plane of the map in camera frame points
# (0, 0, 0), (1, 1, 0), (-1, -1, 0) = map plane in map frame coordinates
p1 = [-94.0, -98.0, 0.0]
p2 = [-92.0, -88.0, 0.0]
p3 = [-84.0, -88.0, 0.0]
# p2 = [1.0, -1.0, 0.0]
# p3 = [-1.0, -1.0, 0.0]
# point_marker = self.create_points_marker([p1, p2, p3], "/map")
# self.plane_pub.publish(point_marker)
self.tf_listener.waitForTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0), rospy.Duration(0.5))
(trans,rot) = self.tf_listener.lookupTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0))
self.tfros = tf.TransformerROS()
tf_mat = self.tfros.fromTranslationRotation(trans, rot)
# pts in camera frame coodrds
p1 = list(np.dot(tf_mat, np.array([p1[0], p1[1], p1[2], 1.0])))[:3]
p2 = list(np.dot(tf_mat, np.array([p2[0], p2[1], p2[2], 1.0])))[:3]
p3 = list(np.dot(tf_mat, np.array([p3[0], p3[1], p3[2], 1.0])))[:3]
plane_norm = self.get_plane_normal(p1, p2, p3) ## maybe viz
print "P1 ", p1
print "P2 ", p2
print "P3 ", p3
print "Plane norm: ", plane_norm
point_marker = self.create_points_marker([p1, p2, p3], img_msg.header.frame_id)
self.plane_pub.publish(point_marker)
# need unit vector to define ray to cast onto plane
line_pt = list(unit_vector)
line_norm = self.get_line_normal([0.0,0.0,0.0], np.asarray(line_pt) * 10) # just scale the unit vector for another point, maybe just use both unit vector for the point and the line
inter = self.line_plane_intersection(line_pt, line_norm, p1, plane_norm)
## intersection is in the camera frame...
### tf this into map frame
self.tf_listener.waitForTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0), rospy.Duration(0.5))
(trans,rot) = self.tf_listener.lookupTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0))
self.tfros = tf.TransformerROS()
tf_mat = self.tfros.fromTranslationRotation(trans, rot)
xyz = tuple(np.dot(tf_mat,np.array([inter[0], inter[1], inter[2], 1.0])))[:3]
print "intersection pt: ", xyz
point_marker = self.create_point_marker(xyz, "/map")
self.inter_pub.publish(point_marker)
line_marker = self.create_line_marker([0.0,0.0,0.0], np.asarray(unit_vector) * 30, img_msg.header.frame_id)
self.line_pub.publish(line_marker)
self.ready_for_image = False
self.points = []
class Seeker():
def __init__(self,classifier,cvbridge):
self.classifier = classifier
self.cvbridge = cvbridge
cam_info=rospy.wait_for_message("/camera/rgb/camera_info", CameraInfo, timeout=None)
# print "Waiting for image data"
# while not (len(self.classifier.subscriber.depth_img)>0):
# if not rospy.is_shutdown():
# rospy.sleep(1)
# print self.classifier.subscriber.depth_img
# else: break
# data = self.classifier.subscriber.depth_img #depth image should now match up with whatever rgb_image was last used in the classifier
# img = self.cvbridge.imgmsg_to_cv2(data) #, "32FC1"
# img = np.array(img, dtype=np.float32)
self.img_shape = (480,640) #np.shape(img)
self.imgproc=PC()
self.imgproc.fromCameraInfo(cam_info)
self.rect = np.array([np.empty((0,4))]) #Array of most recent valid rectangles
self.xyz = np.array([np.empty((0,3))]) #Array of most recent valid xyz position
self.vel = np.array([np.zeros(3)])
def seek(self):
t = 0.25
scale = 1.5
# rospy.sleep(1)
rect = self.track(t,scale)
if not np.size(rect)>0:
print "Trying again with larger rectangle"
for i in range(3): #let's try a couple more times...
t += 1
scale = scale * 1.5
rect = self.track(t,scale)
if np.size(rect)>0:
break
if not np.size(rect)>0:
print "seek: Object lost. Re-initializing search"
self.rect = np.array([np.empty((0,4))])
self.xyz = np.array([np.empty((0,3))])
self.vel = np.array([np.zeros(3)])
return
xyz = self.get_xyz(rect)
if len(xyz) > 0:
self.append_history(rect,xyz)
else:
print "Couldn't retrieve depth info. Make sure object is at least 3 ft from camera and unobstructed."
print "Trying again with larger rectangle"
for i in range(3): #let's try a couple more times...
t += 1
scale = scale * 1.5
rect = self.track(t,scale)
if len(rect)>0:
break
if not len(rect)>0:
print "\n ===== seek: Object lost. Re-initializing search \n"
self.rect = np.array([np.empty((0,4))])
self.xyz = np.array([np.empty((0,3))])
self.vel = np.array([np.zeros(3)])
#displaying feed
disp_img = self.classifier.subscriber.rgb_img
disp_img = self.cvbridge.imgmsg_to_cv2(disp_img, "bgr8")
disp_img = np.array(disp_img, dtype=np.uint8)
#overlaying result if available
if len(rect)>0:
# print "rect: ",rect
(x1,y1,x2,y2)=np.int32(rect[0])
cv2.rectangle(disp_img, (x1, y1), (x1+x2, y1+y2), (0, 255, 0), 2)
text = "X: %.1f Y: %.1f Z: %.1f" %(xyz[0],xyz[1],xyz[2])
cv2.putText(disp_img,text, (x1-10,y1-10),cv2.FONT_HERSHEY_PLAIN,1,(0, 255, 0),1)
cv2.imshow("Seek",disp_img)
cv2.waitKey(1)
def calc_avg(self):
avg_rect = np.zeros(4)
avg_xyz = np.zeros(3)
avg_vel = np.zeros(3)
n = len(self.rect)
for i in range(n):
avg_rect += self.rect[i]*(i+1) #greater weight given to more recent values
avg_xyz += self.xyz[i]*(i+1)
if i>0:
vel = (self.xyz[i]-self.xyz[i-1])*i
avg_vel+=vel
avg_rect = np.int32(avg_rect / (n*(n+1)/2))
avg_xyz = avg_xyz / (n*(n+1)/2)
if n>1:
avg_vel = avg_vel / (n*(n+1)/2)
return (avg_rect, avg_xyz, avg_vel)
def track(self,t,scale):
# t represents estimated time elapsed between the last image obtained and the coming one... one might guess 1 second... this can be made adaptive with time history data... [?]
if not len(self.xyz[0])>0:
# print "Seeker: track() called before position history exists"
self.initial_seek() #might as well do so here
return self.rect
(avg_rect, avg_xyz, avg_vel) = self.calc_avg()
guess_xyz = avg_xyz + t * avg_vel
gx,gy,gz = guess_xyz
(g_centroid_x, g_centroid_y) = np.array(self.imgproc.project3dToPixel((gx,gy,gz)))
ax1,ay1,ax2,ay2 = avg_rect
ax2 = ax2 * scale
ay2 = ay2 * scale
a_centroid_x = ax1 + (ax2/2)
a_centroid_y = ay1 + (ay2/2)
new_centroid_x = np.int((a_centroid_x + g_centroid_x)/2) #centroid of new rectangle encompassing both current rectangle and guessed rectangle
new_centroid_y = np.int((a_centroid_y + g_centroid_y)/2)
new_x2 = np.abs(a_centroid_x - g_centroid_x) + ax2 #proposed rectangle width
new_y2 = np.abs(a_centroid_y - g_centroid_y) + ay2 #proposed rectangle height
new_x1 = max(0,new_centroid_x - np.int(new_x2/2)) #make sure new rectangle's top & left edges are within image
new_y1 = max(0,new_centroid_y - np.int(new_y2/2))
new_x2 = min(new_x2, self.img_shape[1] - new_x1) #make sure new rectangle's bottom & right edges are within image
new_y2 = min(new_y2, self.img_shape[0] - new_y1)
new_rect = np.array([new_x1,new_y1,new_x2,new_y2])
# print "track: new_rect: ", new_rect
#######REPLACE THIS WITH SOMETHING FASTER TT__TT
# rect = self.classifier.seg_search(new_rect)
rect = self.seg_track(new_rect)
# print "track: rect: ",rect
return rect
def append_history(self,rect,xyz):
# newest valid values are last in the list
# print "Appending: ",rect, xyz
# print "Current Rect, Current xyz: ", self.rect,self.xyz
rect = np.array(rect).reshape(4)
xyz = np.array(xyz).reshape(3)
self.rect = np.append(self.rect,[rect],0)
self.xyz = np.append(self.xyz,[xyz],0)
# print "Stored Rect, Stored xyz: ", self.rect,self.xyz
if len(self.rect)>5: #both history arrays should be of the same length, so no need to check both
#remove values beyond the 5th
self.rect = self.rect[-5:]
self.xyz = self.xyz[-5:]
def initial_seek(self):
print "Searching for object. Don't move!"
rect = self.classifier.initial_search_timed(2)
if len(rect) > 0:
# print "Rect: ", rect
xyz = self.get_xyz(rect)
if len(xyz) > 0:
# print xyz
#initializes history
self.rect = np.array([rect])
self.xyz = np.array([xyz])
self.vel = np.array([np.zeros(3)])
else:
print "Couldn't retrieve depth info. Make sure object is at least 3 ft from camera and unobstructed."
def seg_track(self,seg_rect):
# A much faster but potentially less accurate tracking function using depth data instead of trained classifier
# Given rect coord potentially containing object, returns rect around object if found
(rgb_frame,depth_frame) = self.classifier.get_feed() #retrieves updated rgb and depth images
# disp_frame = copy(rgb_frame)
# cv2.rectangle(disp_frame, (x1, y1), (x1+x2, y1+y2), (0, 255, 0), 2)
# cv2.imshow('Searching',disp_frame)
# cv2.waitKey(0)
seg_rect = np.array(seg_rect).reshape(4)
x1,y1,x2,y2 = seg_rect
depth_seg = depth_frame[y1:(y1+y2),x1:(x1+x2)]
(avg_rect, avg_xyz, avg_vel) = self.calc_avg()
if (avg_vel[2]>0):
depth_min = avg_xyz[2]
depth_max = depth_min + avg_vel[2] * 0.5 #predicts how far the object may have travelled in... 0.5 sec [?]
else:
depth_max = avg_xyz[2]
depth_min = depth_max + avg_vel[2] * 0.5
mask = np.nan_to_num(depth_seg)
ind = np.nonzero(( (mask>(depth_min-0.05)) & (mask<(depth_max+0.05)) ))
mask = np.zeros(np.shape(mask))
mask[ind] = 1
kernel = np.ones((10,10),np.uint8)
# mask = cv2.erode(mask,kernel,iterations = 1)
# mask = cv2.dilate(mask,kernel,iterations = 1)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) #clean noise
# cv2.imshow('Searching',mask)
# cv2.waitKey(0)
ind = np.nonzero(mask)
if (len(ind[0])==0):
return np.empty((0,4))
# calculation accuracy rely on there being no other objects of similar depth in the segment
dx2 = np.int32(0.5 * ((max(ind[1]) - min(ind[1])) + avg_rect[2]))
dy2 = np.int32(0.5 * ((max(ind[0]) - min(ind[0])) + avg_rect[3]))
dcentroid_x = np.int32(np.mean(ind[1]))
dcentroid_y = np.int32(np.mean(ind[0]))
# dx2 = dy2 = (dx2+dy2)/2
dx1 = max(dcentroid_x - (dx2/2),0)
dy1 = max(dcentroid_y - (dy2/2),0)
dx2 = min(dx2,self.img_shape[1]-dx1)
dy2 = min(dy2,self.img_shape[0]-dy1)
sdx1 = x1+dx1
sdy1 = y1+dy1
dseg = rgb_frame[sdy1:(sdy1+dy2),sdx1:(sdx1+dx2)]
# cv2.imshow('Searching',dseg)
# cv2.waitKey(0)
#if the image segment doesn't pass the color histogram test, object isn't in the segment
if self.classifier.hist_test.compare(dseg):
rect = np.array([[sdx1,sdy1,dx2,dy2]])
else:
rect = np.empty((0,4))
return rect
def get_xyz(self,rect):
# data = rospy.wait_for_message("/camera/depth_registered/hw_registered/image_rect", Image, timeout=None)
data = self.classifier.subscriber.depth_img #depth image should now match up with whatever rgb_image was last used in the classifier
img = self.cvbridge.imgmsg_to_cv2(data) #, "32FC1"
img = np.array(img, dtype=np.float32)
rect = np.array(rect).reshape(4,)
x1,y1,x2,y2 = rect
frame = img[y1:(y1+y2),x1:(x1+x2)]
mask = np.nan_to_num(frame)
#takes mean of center pixel values, assuming object exists there and there's no hole in the object's depth map
seg = mask[int(0.45*y2):int(0.55*y2),int(0.45*x2):int(0.55*x2)]
seg_mean = np.mean(seg) #approximate depth value of the object. Take mean of remaining object pixels
if (seg_mean > 0.25): #if there's no hole in the map. Minimum operating distance is about 0.5 meters
seg_std = np.std(seg)
ind = np.nonzero(np.abs(mask-seg_mean) < ((1.5 * seg_std) + 0.025)) # Valid region should be within one standard deviation of the approximate mean[?]
if np.size(ind) == 0:
return np.empty((0,3))
mask_depth = mask[ind]
obj_depth = np.mean(mask_depth)
else: #backup method in case hole exists
ind_depth_calc = np.nonzero((mask>0.25) & (mask<5)) #
mask_depth = mask[ind_depth_calc]
mask_mean = np.mean(mask_depth)
mask_std = np.std(mask_depth)
ind = np.nonzero((np.abs(mask-mask_mean) < (max(0.25,0.5*mask_std))))
if np.size(ind) < np.int(0.5 *(np.size(mask))):
return np.empty((0,3))
obj_depth = np.mean(mask[ind]) # mean depth value of pixels representing object
# mask_depth = np.uint8(mask_depth*(255/np.max(mask_depth))) #normalize to greyscale
# mask = np.uint8(mask*(255/np.max(mask_depth)))
# retval,mask = cv2.threshold(mask,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU) #threshold was designed for greyscale, but this works too.
# kernel = np.ones((5,5),np.uint8)
# mask = cv2.erode(mask,kernel,iterations = 1) #[?]
# ind = np.nonzero(mask)
# print "Object Depth: ", obj_depth
row = ind[0]+y1
column = ind[1]+x1
# blah = np.zeros(np.shape(img))
# blah[(row,column)] = 255
# cv2.imshow("Blah",blah)
# cv2.waitKey(1)
centroid_y = np.mean(row)
centroid_x = np.mean(column)
obj_coord = np.array(self.imgproc.projectPixelTo3dRay((centroid_x,centroid_y)))
scale=obj_depth/obj_coord[2] #scale the unit vector according to measured depth
# print "Centroid y,x,ray,depth: ", centroid_y,centroid_x,obj_coord,obj_depth
obj_coord_sc=obj_coord*scale
return obj_coord_sc
class MarkerFinder():
def __init__(self):
self.tf_listener = tf.TransformListener()
self.search = False
self.last_image = None
self.last_image_timestamp = None
self.last_draw_image = None
# This may need to be changed if you want to use a different image feed.
self.image_sub = sub8_ros_tools.Image_Subscriber('/down/left/image_raw', self.image_cb)
self.image_pub = sub8_ros_tools.Image_Publisher("vision/channel_marker/target_info")
self.toggle = rospy.Service('vision/channel_marker/search', SetBool, self.toggle_search)
self.cam = PinholeCameraModel()
self.cam.fromCameraInfo(self.image_sub.wait_for_camera_info())
self.rviz = rviz.RvizVisualizer()
# self.occ_grid = MarkerOccGrid(self.image_sub, grid_res=.05, grid_width=500, grid_height=500,
# grid_starting_pose=Pose2D(x=250, y=250, theta=0))
if rospy.get_param("/orange_markers/use_boost"):
path = os.path.join(rospack.get_path('sub8_perception'),
'ml_classifiers/marker/' + rospy.get_param("/orange_markers/marker_classifier"))
self.boost = cv2.Boost()
rospy.loginfo("MARKER - Loading classifier from {}".format(path))
self.boost.load(path)
rospy.loginfo("MARKER - Classifier for marker loaded.")
else:
self.lower = np.array(rospy.get_param('/color/channel_marker/hsv_low'))
self.upper = np.array(rospy.get_param('/color/channel_marker/hsv_high'))
self.pose_service = rospy.Service("vision/channel_marker/pose", VisionRequest, self.request_marker)
self.kernel = np.ones((15, 15), np.uint8)
# Occasional status publisher
self.timer = rospy.Timer(rospy.Duration(1), self.publish_target_info)
print "MARKER - Got no patience for sittin' around!"
def toggle_search(self, srv):
if srv.data:
rospy.loginfo("MARKER - Looking for markers now.")
self.search = True
else:
rospy.loginfo("MARKER - Done looking for markers.")
self.search = False
return SetBoolResponse(success=srv.data)
def publish_target_info(self, *args):
if not self.search or self.last_image is None:
return
markers = self.find_marker(np.copy(self.last_image))
#self.occ_grid.update_grid(self.last_image_timestamp)
#self.occ_grid.add_marker(markers, self.last_image_timestamp)
if self.last_draw_image is not None: #and (markers is not None):
self.image_pub.publish(np.uint8(self.last_draw_image))
def image_cb(self, image):
'''Hang on to last image and when it was taken.'''
self.last_image = image
self.last_image_timestamp = self.image_sub.last_image_time
def calculate_threshold(self, img, agression=.5):
histr = cv2.calcHist([img], [0], None, [179], [0, 179])
threshold = np.uint8((179 - np.argmax(histr)) * agression)
return threshold
def get_2d_pose(self, mask):
# estimate covariance matrix and get corresponding eigenvectors
wh = np.where(mask)[::-1]
cov = np.cov(wh)
eig_vals, eig_vects = np.linalg.eig(cov)
# use index of max eigenvalue to find max eigenvector
i = np.argmax(eig_vals)
max_eigv = eig_vects[:, i] * np.sqrt(eig_vals[i])
# flip indices to find min eigenvector
min_eigv = eig_vects[:, 1 - i] * np.sqrt(eig_vals[1 - i])
# define center of pipe
center = np.average(wh, axis=1)
# define vertical vector (sub's current direction)
vert_vect = np.array([0.0, -1.0])
if max_eigv[1] > 0:
max_eigv = -max_eigv
min_eigv = -min_eigv
num = np.cross(max_eigv, vert_vect)
denom = np.linalg.norm(max_eigv) * np.linalg.norm(vert_vect)
angle_rad = np.arcsin(num / denom)
return center, angle_rad, [max_eigv, min_eigv]
def find_marker(self, img):
#img[:, -100:] = 0
img = cv2.GaussianBlur(img, (7, 7), 15)
last_image_timestamp = self.last_image_timestamp
if rospy.get_param("/orange_markers/use_boost"):
some_observations = machine_learning.boost.observe(img)
prediction = [int(x) for x in [self.boost.predict(obs) for obs in some_observations]]
mask = np.reshape(prediction, img[:, :, 2].shape).astype(np.uint8) * 255
else:
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, self.lower, self.upper)
kernel = np.ones((5,5),np.uint8)
mask = cv2.dilate(mask, kernel, iterations = 1)
mask = cv2.erode(mask, kernel, iterations = 2)
#mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, self.kernel)
contours, _ = cv2.findContours(np.copy(mask), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) < 1:
rospy.logwarn("MARKER - No marker found.")
return None
# Find biggest area contour
self.last_draw_image = np.dstack([mask] * 3)
areas = [cv2.contourArea(c) for c in contours]
max_index = np.argmax(areas)
cnt = contours[max_index]
# Draw a miniAreaRect around the contour and find the area of that.
rect = cv2.minAreaRect(cnt)
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
mask = np.zeros(shape=mask.shape)
cv2.drawContours(self.last_draw_image, [box], 0, (0, 128, 255), 2)
cv2.drawContours(mask, [box], 0, 255, -1)
rect_area = cv2.contourArea(box)
center, angle_rad, [max_eigv, min_eigv] = self.get_2d_pose(mask)
cv2.line(self.last_draw_image, tuple(np.int0(center)), tuple(np.int0(center + (2 * max_eigv))), (0, 255, 30), 2)
cv2.line(self.last_draw_image, tuple(np.int0(center)), tuple(np.int0(center + (2 * min_eigv))), (0, 30, 255), 2)
# Check if the box is too big or small.
xy_position, height = self.get_tf(timestamp=last_image_timestamp)
expected_area = self.calculate_marker_area(height)
print "{} < {} < {} ({})".format(expected_area * .2, rect_area, expected_area * 2, expected_area)
if expected_area * .5 < rect_area < expected_area * 2:
#cv2.drawContours(self.last_draw_image, [box], 0, (255, 255, 255), -1)
self.rviz.draw_ray_3d(center, self.cam, np.array([1, .5, 0, 1]), frame='/downward',
_id=5, length=height, timestamp=last_image_timestamp)
else:
angle_rad = 0
max_eigv = np.array([0, -20])
min_eigv = np.array([-20, 0])
#cv2.drawContours(self.last_draw_image, [box], 0, (255, 0, 30), -1)
rospy.logwarn("MARKER - Size out of bounds!")
self.rviz.draw_ray_3d(center, self.cam, np.array([1, .5, 0, 1]), frame='/downward',
_id=5, length=height, timestamp=last_image_timestamp)
# Convert to a 3d pose to move the sub to.
abs_position = self.transform_px_to_m(center, last_image_timestamp)
q_rot = tf.transformations.quaternion_from_euler(0, 0, angle_rad)
return numpy_quat_pair_to_pose(abs_position, q_rot)
def request_marker(self, data):
if self.last_image is None:
return False # Fail if we have no images cached
timestamp = self.last_image_timestamp
goal_pose = self.find_marker(self.last_image)
found = (goal_pose is not None)
if not found:
resp = VisionRequestResponse(
found=found
)
else:
resp = VisionRequestResponse(
pose=PoseStamped(
header=Header(
stamp=timestamp,
frame_id='/down_camera'),
pose=goal_pose
),
found=found
)
return resp
def get_tf(self, timestamp=None, get_rotation=False):
'''
x_y position, height in meters and quat rotation of the sub if requested
'''
if timestamp is None:
timestamp = rospy.Time()
self.tf_listener.waitForTransform("/map", "/downward", timestamp, rospy.Duration(5.0))
trans, rot = self.tf_listener.lookupTransform("/map", "/downward", timestamp)
x_y_position = trans[:2]
self.tf_listener.waitForTransform("/ground", "/downward", timestamp, rospy.Duration(5.0))
trans, _ = self.tf_listener.lookupTransform("/ground", "/downward", timestamp)
height = np.nan_to_num(trans[2])
x_y_position = np.nan_to_num(x_y_position)
if get_rotation:
return x_y_position, rot, height
return x_y_position, height
def transform_px_to_m(self, m_position, timestamp):
'''
Finds the absolute position of the marker in meters.
'''
xy_position, q, height = self.get_tf(timestamp, get_rotation=True)
dir_vector = unit_vector(np.array([self.cam.cx(), self.cam.cy()]) - m_position)
cam_rotation = tf.transformations.euler_from_quaternion(q)[2] + np.pi / 2
print "MARKER - dir_vector:", dir_vector
print "MARKER - cam_rotation:", cam_rotation
dir_vector = np.dot(dir_vector, make_2D_rotation(cam_rotation))
# Calculate distance from middle of frame to marker in meters.
magnitude = self.calculate_visual_radius(height, second_point=m_position)
abs_position = np.append(xy_position + dir_vector[::-1] * magnitude, -SEARCH_DEPTH)
return abs_position
def calculate_visual_radius(self, height, second_point=None):
'''
Draws rays to find the radius of the FOV of the camera in meters.
It also can work to find the distance between two planar points some distance from the camera.
'''
mid_ray = np.array([0, 0, 1])
if second_point is None:
if self.cam.cy() < self.cam.cx():
second_point = np.array([self.cam.cx(), 0])
else:
second_point = np.array([0, self.cam.cy()])
edge_ray = unit_vector(self.cam.projectPixelTo3dRay(second_point))
# Calculate angle between vectors and use that to find r
theta = np.arccos(np.dot(mid_ray, edge_ray))
return np.tan(theta) * height
def calculate_marker_area(self, height):
'''
What we really don't want is to find markers that are on the edge since the direction and center of
the marker are off.
'''
MARKER_LENGTH = 1.22 # m
MARKER_WIDTH = .1524 # m
# Get m/px on the ground floor.
m = self.calculate_visual_radius(height)
if self.cam.cy() < self.cam.cx():
px = self.cam.cy()
else:
px = self.cam.cx()
m_px = m / px
marker_area_m = MARKER_WIDTH * MARKER_LENGTH
marker_area_px = marker_area_m / (m_px ** 2)
return marker_area_px
class InteractiveDataLabeler:
"""
Handles data labelling for a generic sensor:
Cameras: Fully automated labelling. Periodically runs a chessboard detection on the newly received image.
LaserScans: Semi-automated labelling. An rviz interactive marker is placed on the laser cluster which contains
the calibration pattern, and the pattern is tracked from there onward.
PointCloud2: #TODO Tiago Madeira can you complete?
"""
def __init__(self, server, menu_handler, sensor_dict, marker_scale,
calib_pattern):
"""
Class constructor. Initializes several variables and ros stuff.
:param server: an interactive marker server
:param menu_handler: an interactive MenuHandler
:param sensor_dict: A dictionary that describes the sensor
:param marker_scale: scale of the markers to be drawn
:param chess_numx: chessboard size in x
:param chess_numy: chessboard size in y
"""
print('Creating an InteractiveDataLabeler for sensor ' +
str(sensor_dict['_name']))
# Store variables to class attributes
self.server = server
self.menu_handler = menu_handler
self.name = sensor_dict['_name']
self.parent = sensor_dict['parent']
self.topic = sensor_dict['topic']
self.marker_scale = marker_scale
self.received_first_msg = False
self.labels = {'detected': False, 'idxs': []}
self.lock = threading.Lock()
# self.calib_pattern = calib_pattern
if calib_pattern['pattern_type'] == 'chessboard':
self.pattern = patterns.ChessboardPattern(
calib_pattern['dimension'], calib_pattern['size'])
elif calib_pattern['pattern_type'] == 'charuco':
self.pattern = patterns.CharucoPattern(calib_pattern['dimension'],
calib_pattern['size'],
calib_pattern['inner_size'],
calib_pattern['dictionary'])
print(calib_pattern['dictionary'])
else:
print("Unknown pattern type '{}'".format(
calib_pattern['pattern_type']))
sys.exit(1)
# Get the type of message from the message topic of the sensor data, which is given as input. The message
# type is used to define which labelling technique is used.
self.msg_type_str, self.msg_type = atom_core.utilities.getMessageTypeFromTopic(
self.topic)
print('msg_type_str is = ' + str(self.msg_type_str))
# Handle the interactive labelling of data differently according to the sensor message types.
if self.msg_type_str in ['LaserScan']:
# TODO parameters given from a command line input?
self.threshold = 0.2 # pt to pt distance to create new cluster (param only for 2D LIDAR labelling)
self.minimum_range_value = 0.3 # distance to assume range value valid (param only for 2D LIDAR labelling)
self.publisher_selected_points = rospy.Publisher(
self.topic + '/labeled',
sensor_msgs.msg.PointCloud2,
queue_size=0) # publish a point cloud with the points
# in the selected cluster
self.publisher_clusters = rospy.Publisher(
self.topic + '/clusters',
sensor_msgs.msg.PointCloud2,
queue_size=0) # publish a point cloud with coloured clusters
self.createInteractiveMarker(
) # interactive marker to label the calibration pattern cluster (one time)
print('Created interactive marker for laser scans.')
elif self.msg_type_str in ['Image']:
self.bridge = CvBridge(
) # a CvBridge structure is needed to convert opencv images to ros messages.
self.publisher_labelled_image = rospy.Publisher(
self.topic + '/labeled', sensor_msgs.msg.Image,
queue_size=0) # publish
# images with the detected chessboard overlaid onto the image.
elif self.msg_type_str in [
'PointCloud2TIAGO'
]: # TODO, this will have to be revised later on Check #44
self.publisher_selected_points = rospy.Publisher(
self.topic + '/labeled',
sensor_msgs.msg.PointCloud2,
queue_size=0) # publish a point cloud with the points
self.createInteractiveMarkerRGBD(
) # interactive marker to label the calibration pattern cluster (one time)
self.bridge = CvBridge()
self.publisher_labelled_depth_image = rospy.Publisher(
self.topic + '/depth_image_labelled',
sensor_msgs.msg.Image,
queue_size=0) # publish
self.cam_model = PinholeCameraModel()
topic_name = os.path.dirname(self.topic) + '/camera_info'
rospy.loginfo('Waiting for for camera info message on topic' +
str(topic_name))
camera_info = rospy.wait_for_message(
'/top_center_rgbd_camera/depth/camera_info', CameraInfo)
print('... received!')
self.cam_model.fromCameraInfo(camera_info)
print('Created interactive marker for point clouds.')
elif self.msg_type_str in ['PointCloud2'
]: # Velodyne data (Andre Aguiar)
self.publisher_selected_points = rospy.Publisher(
self.topic + '/labeled',
sensor_msgs.msg.PointCloud2,
queue_size=0) # publish a point cloud with the points
self.createInteractiveMarkerRGBD(
x=0.804, y=0.298, z=-0.109
) # interactive marker to label the calibration pattern
# cluster (one time)
# Labeler definitions
# Hessian plane coefficients
self.A = 0
self.B = 0
self.C = 0
self.D = 0
self.n_inliers = 0 # RANSAC number of inliers initialization
self.number_iterations = 30 # RANSAC number of iterations
self.ransac_threshold = 0.02 # RANSAC point-to-plane distance threshold to consider inliers
# Chessboard point tracker distance threshold
self.tracker_threshold = math.sqrt(((calib_pattern['dimension']['x'] - 1) * calib_pattern['size']) ** 2 + \
((calib_pattern['dimension']['y'] - 1) * calib_pattern['size']) ** 2)
print('Created interactive marker for point clouds.')
else:
# We handle only know message types
raise ValueError('Message type ' + self.msg_type_str +
' for topic ' + self.topic +
'is of an unknown type.')
# self.publisher = rospy.Publisher(self.topic + '/labeled', self.msg_type, queue_size=0)
# Subscribe to the message topic containing sensor data
print(self.msg_type)
self.subscriber = rospy.Subscriber(self.topic,
self.msg_type,
self.sensorDataReceivedCallback,
queue_size=None)
def sensorDataReceivedCallback(self, msg):
self.lock.acquire(
) # use semaphores to make sure the data is not being written on two sides simultaneously
self.msg = msg # make a local copy of sensor data
# now = rospy.Time.now()
self.labelData # label the data
# rospy.loginfo('Labelling data for ' + self.name + ' took ' + str((rospy.Time.now() - now).to_sec()) + ' secs.')
self.lock.release() # release lock
@property
def labelData(self):
# Detected and idxs values to False and [], to make sure we are not using information from a previous labelling
self.labels['detected'] = False
self.labels['idxs'] = []
# Labelling process dependent of the sensor type
if self.msg_type_str == 'LaserScan': # 2D LIDARS -------------------------------------
# For 2D LIDARS the process is the following: First cluster all the range data into clusters. Then,
# associate one of the clusters with the calibration pattern by selecting the cluster which is closest to
# the rviz interactive marker.
clusters = [] # initialize cluster list to empty
cluster_counter = 0 # init counter
points = [] # init points
# Compute cartesian coordinates
xs, ys = atom_core.utilities.laser_scan_msg_to_xy(self.msg)
# Clustering:
first_iteration = True
for idx, r in enumerate(self.msg.ranges):
# Skip if either this point or the previous have range smaller than minimum_range_value
if r < self.minimum_range_value or self.msg.ranges[
idx - 1] < self.minimum_range_value:
continue
if first_iteration: # if first iteration, create a new cluster
clusters.append(LaserScanCluster(cluster_counter, idx))
first_iteration = False
else: # check if new point belongs to current cluster, create new cluster if not
x = xs[clusters[-1].idxs[
-1]] # x coordinate of last point of last cluster
y = ys[clusters[-1].idxs[
-1]] # y coordinate of last point of last cluster
distance = math.sqrt((xs[idx] - x)**2 + (ys[idx] - y)**2)
if distance > self.threshold: # if distance larger than threshold, create new cluster
cluster_counter += 1
clusters.append(LaserScanCluster(cluster_counter, idx))
else: # same cluster, push this point into the same cluster
clusters[-1].pushIdx(idx)
# Association stage: find out which cluster is closer to the marker
x_marker, y_marker = self.marker.pose.position.x, self.marker.pose.position.y # interactive marker pose
idx_closest_cluster = 0
min_dist = sys.maxint
for cluster_idx, cluster in enumerate(
clusters): # cycle all clusters
for idx in cluster.idxs: # cycle each point in the cluster
x, y = xs[idx], ys[idx]
dist = math.sqrt((x_marker - x)**2 + (y_marker - y)**2)
if dist < min_dist:
idx_closest_cluster = cluster_idx
min_dist = dist
closest_cluster = clusters[idx_closest_cluster]
# Find the coordinate of the middle point in the closest cluster and bring the marker to that point
x_sum, y_sum = 0, 0
for idx in closest_cluster.idxs:
x_sum += xs[idx]
y_sum += ys[idx]
self.marker.pose.position.x = x_sum / float(
len(closest_cluster.idxs))
self.marker.pose.position.y = y_sum / float(
len(closest_cluster.idxs))
self.marker.pose.position.z = 0
self.menu_handler.reApply(self.server)
self.server.applyChanges()
# Update the dictionary with the labels
self.labels['detected'] = True
percentage_points_to_remove = 0.0 # remove x% of data from each side
number_of_idxs = len(clusters[idx_closest_cluster].idxs)
idxs_to_remove = int(percentage_points_to_remove *
float(number_of_idxs))
clusters[idx_closest_cluster].idxs_filtered = clusters[
idx_closest_cluster].idxs[idxs_to_remove:number_of_idxs -
idxs_to_remove]
self.labels['idxs'] = clusters[idx_closest_cluster].idxs_filtered
# Create and publish point cloud message with the colored clusters (just for debugging)
cmap = cm.prism(np.linspace(0, 1, len(clusters)))
points = []
z, a = 0, 255
for cluster in clusters:
for idx in cluster.idxs:
x, y = xs[idx], ys[idx]
r, g, b = int(cmap[cluster.cluster_count, 0] * 255.0), \
int(cmap[cluster.cluster_count, 1] * 255.0), \
int(cmap[cluster.cluster_count, 2] * 255.0)
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r,
a))[0]
pt = [x, y, z, rgb]
points.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgba', 12, PointField.UINT32, 1)
]
header = Header()
header.frame_id = self.parent
header.stamp = self.msg.header.stamp
pc_msg = point_cloud2.create_cloud(header, fields, points)
self.publisher_clusters.publish(pc_msg)
# Create and publish point cloud message containing only the selected calibration pattern points
points = []
for idx in clusters[idx_closest_cluster].idxs_filtered:
x_marker, y_marker, z_marker = xs[idx], ys[idx], 0
r = int(0 * 255.0)
g = int(0 * 255.0)
b = int(1 * 255.0)
a = 255
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [x_marker, y_marker, z_marker, rgb]
points.append(pt)
pc_msg = point_cloud2.create_cloud(header, fields, points)
self.publisher_selected_points.publish(pc_msg)
elif self.msg_type_str == 'Image': # Cameras -------------------------------------------
# Convert to opencv image and save image to disk
image = self.bridge.imgmsg_to_cv2(self.msg, "bgr8")
result = self.pattern.detect(image, equalize_histogram=True)
if result['detected']:
c = []
if result.has_key('ids'):
# The charuco pattern also return an ID for each keypoint.
# We can use this information for partial detections.
for idx, corner in enumerate(result['keypoints']):
c.append({
'x': float(corner[0][0]),
'y': float(corner[0][1]),
'id': result['ids'][idx]
})
else:
for corner in result['keypoints']:
c.append({
'x': float(corner[0][0]),
'y': float(corner[0][1])
})
x = int(round(c[0]['x']))
y = int(round(c[0]['y']))
cv2.line(image, (x, y), (x, y), (0, 255, 255), 20)
# Update the dictionary with the labels
self.labels['detected'] = True
self.labels['idxs'] = c
# For visual debugging
self.pattern.drawKeypoints(image, result)
msg_out = self.bridge.cv2_to_imgmsg(image, encoding="passthrough")
msg_out.header.stamp = self.msg.header.stamp
msg_out.header.frame_id = self.msg.header.frame_id
self.publisher_labelled_image.publish(msg_out)
elif self.msg_type_str == 'PointCloud2TIAGO': # RGB-D pointcloud -------------------------------------------
# TODO, this will have to be revised later on Check #44
# print("Found point cloud!")
tall = rospy.Time.now()
# Get 3D coords
t = rospy.Time.now()
# points = pc2.read_points_list(self.msg, skip_nans=False, field_names=("x", "y", "z"))
print('0. took ' + str((rospy.Time.now() - t).to_sec()))
# Get the marker position
x_marker, y_marker, z_marker = self.marker.pose.position.x, self.marker.pose.position.y, self.marker.pose.position.z # interactive marker pose
t = rospy.Time.now()
# Project points
print('x_marker=' + str(x_marker))
print('y_marker=' + str(y_marker))
print('z_marker=' + str(z_marker))
seed_point = self.cam_model.project3dToPixel(
(x_marker, y_marker, z_marker))
print('seed_point = ' + str(seed_point))
if np.isnan(
seed_point[0]
): # something went wrong, reposition marker on initial position and return
self.marker.pose.position.x = 0
self.marker.pose.position.y = 0
self.marker.pose.position.z = 4
self.menu_handler.reApply(self.server)
self.server.applyChanges()
rospy.logwarn(
'Could not project pixel, putting marker in home position.'
)
return
seed_point = (int(round(seed_point[0])), int(round(seed_point[1])))
# Check if projection is inside the image
x = seed_point[0]
y = seed_point[1]
if x < 0 or x >= self.cam_model.width or y < 0 or y >= self.cam_model.height:
rospy.logwarn(
'Projection of point is outside of image. Not labelling point cloud.'
)
return
print('1. took ' + str((rospy.Time.now() - t).to_sec()))
t = rospy.Time.now()
# Wait for depth image message
imgmsg = rospy.wait_for_message(
'/top_center_rgbd_camera/depth/image_rect', Image)
print('2. took ' + str((rospy.Time.now() - t).to_sec()))
t = rospy.Time.now()
# img = self.bridge.imgmsg_to_cv2(imgmsg, desired_encoding="8UC1")
img_raw = self.bridge.imgmsg_to_cv2(imgmsg,
desired_encoding="passthrough")
img = deepcopy(img_raw)
img_float = img.astype(np.float32)
img_float = img_float
h, w = img.shape
# print('img type = ' + str(img.dtype))
# print('img_float type = ' + str(img_float.dtype))
# print('img_float shape = ' + str(img_float.shape))
mask = np.zeros((h + 2, w + 2, 1), np.uint8)
# mask[seed_point[1] - 2:seed_point[1] + 2, seed_point[0] - 2:seed_point[0] + 2] = 255
# PCA + Consensus + FloodFill ------------------
# get 10 points around the seed
# seed = {'x': seed_point[0], 'y': seed_point[1]}
# pts = []
# pts.append({'x': seed['x'], 'y': seed['y'] - 10}) # top neighbor
# pts.append({'x': seed['x'], 'y': seed['y'] + 10}) # bottom neighbor
# pts.append({'x': seed['x'] - 1, 'y': seed['y']}) # left neighbor
# pts.append({'x': seed['x'] + 1, 'y': seed['y']}) # right neighbor
#
# def fitPlaneLTSQ(XYZ):
# (rows, cols) = XYZ.shape
# G = np.ones((rows, 3))
# G[:, 0] = XYZ[:, 0] # X
# G[:, 1] = XYZ[:, 1] # Y
# Z = XYZ[:, 2]
# (a, b, c), resid, rank, s = np.linalg.lstsq(G, Z)
# normal = (a, b, -1)
# nn = np.linalg.norm(normal)
# normal = normal / nn
# return (c, normal)
#
# data = np.random.randn(100, 3) / 3
# data[:, 2] /= 10
# c, normal = fitPlaneLTSQ(data)
# out flood fill ------------------
# to_visit = [{'x': seed_point[0], 'y': seed_point[1]}]
# # filled = []
# threshold = 0.05
# filled_img = np.zeros((h, w), dtype=np.bool)
# visited_img = np.zeros((h, w), dtype=np.bool)
#
# def isInsideBox(p, min_x, max_x, min_y, max_y):
# if min_x <= p['x'] < max_x and min_y <= p['y'] < max_y:
# return True
# else:
# return False
#
# def getNotVisitedNeighbors(p, min_x, max_x, min_y, max_y, img):
# neighbors = []
# tmp_neighbors = []
# tmp_neighbors.append({'x': p['x'], 'y': p['y'] - 1}) # top neighbor
# tmp_neighbors.append({'x': p['x'], 'y': p['y'] + 1}) # bottom neighbor
# tmp_neighbors.append({'x': p['x'] - 1, 'y': p['y']}) # left neighbor
# tmp_neighbors.append({'x': p['x'] + 1, 'y': p['y']}) # right neighbor
#
# for idx, n in enumerate(tmp_neighbors):
# if isInsideBox(n, min_x, max_x, min_y, max_y) and not img[n['y'], n['x']] == True:
# neighbors.append(n)
#
# return neighbors
#
# cv2.namedWindow('Filled', cv2.WINDOW_NORMAL)
# cv2.namedWindow('Visited', cv2.WINDOW_NORMAL)
# cv2.namedWindow('To Visit', cv2.WINDOW_NORMAL)
# while to_visit != []:
# p = to_visit[0]
# # print('Visiting ' + str(p))
# range_p = img_float[p['y'], p['x']]
# to_visit.pop(0) # remove p from to_visit
# # filled.append(p) # append p to filled
# filled_img[p['y'], p['x']] = True
# # print(filled)
#
# # compute neighbors of this point
# neighbors = getNotVisitedNeighbors(p, 0, w, 0, h, visited_img)
#
# # print('neighbors ' + str(neighbors))
#
# for n in neighbors: # test if should propagate to neighbors
# range_n = img_float[n['y'], n['x']]
# visited_img[n['y'], n['x']] = True
#
# if abs(range_n - range_p) <= threshold:
# # if not n in to_visit:
# to_visit.append(n)
#
# # Create the mask image
# to_visit_img = np.zeros((h, w), dtype=np.bool)
# for p in to_visit:
# to_visit_img[p['y'], p['x']] = True
#
#
# # print('To_visit ' + str(to_visit))
#
# cv2.imshow('Filled', filled_img.astype(np.uint8) * 255)
# cv2.imshow('Visited', visited_img.astype(np.uint8) * 255)
# cv2.imshow('To Visit', to_visit_img.astype(np.uint8) * 255)
# key = cv2.waitKey(5)
# --------------------------------
img_float2 = deepcopy(img_float)
cv2.floodFill(
img_float2, mask, seed_point, 128, 80, 80, 8 | (128 << 8)
| cv2.FLOODFILL_MASK_ONLY | cv2.FLOODFILL_FIXED_RANGE)
# Switch coords of seed point
# mask[seed_point[1]-2:seed_point[1]+2, seed_point[0]-2:seed_point[0]+2] = 255
tmpmask = mask[1:h + 1, 1:w + 1]
cv2.namedWindow('tmpmask', cv2.WINDOW_NORMAL)
cv2.imshow('tmpmask', tmpmask)
def onMouse(event, x, y, flags, param):
print("x = " + str(x) + ' y = ' + str(y) + ' value = ' +
str(img_float2[y, x]))
cv2.namedWindow('float', cv2.WINDOW_GUI_EXPANDED)
cv2.setMouseCallback('float', onMouse, param=None)
cv2.imshow('float', img_raw)
key = cv2.waitKey(0)
print('3. took ' + str((rospy.Time.now() - t).to_sec()))
t = rospy.Time.now()
# calculate moments of binary image
M = cv2.moments(tmpmask)
self.labels['detected'] = True
print('4. took ' + str((rospy.Time.now() - t).to_sec()))
t = rospy.Time.now()
if M["m00"] != 0:
# calculate x,y coordinate of center
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
red = deepcopy(img)
# bmask = tmpmask.astype(np.bool)
print(tmpmask.shape)
tmpmask = np.reshape(tmpmask, (480, 640))
print(img.shape)
red[tmpmask != 0] = red[tmpmask != 0] + 10000
img = cv2.merge((img, img, red))
img[cY - 2:cY + 2, cX - 2:cX + 2, 1] = 30000
img[seed_point[1] - 2:seed_point[1] + 2,
seed_point[0] - 2:seed_point[0] + 2, 0] = 30000
# img[100:400, 20:150] = 255
cv2.imshow("mask", img)
cv2.waitKey(5)
# msg_out = self.bridge.cv2_to_imgmsg(showcenter, encoding="passthrough")
# msg_out.header.stamp = self.msg.header.stamp
# msg_out.header.frame_id = self.msg.header.frame_id
# self.publisher_labelled_depth_image.publish(msg_out)
# coords = points[cY * 640 + cX]
# print('coords' + str(coords))
ray = self.cam_model.projectPixelTo3dRay((cX, cY))
print('ray' + str(ray))
print('img' + str(img_float.shape))
print(type(cX))
print(type(cY))
print(type(ray))
dist = float(img_float[cX, cY])
print('dist = ' + str(dist))
x = ray[0] * dist
y = ray[1] * dist
z = ray[2] * dist
print('xyz = ' + str(x) + ' ' + str(y) + ' ' + str(z))
# if not math.isnan(coords[0]):
# self.marker.pose.position.x = coords[0]
# self.marker.pose.position.y = coords[1]
# self.marker.pose.position.z = coords[2]
# self.menu_handler.reApply(self.server)
# self.server.applyChanges()
if dist > 0.1:
# self.marker.pose.position.x = x
# self.marker.pose.position.y = y
# self.marker.pose.position.z = z
# self.menu_handler.reApply(self.server)
# self.server.applyChanges()
pass
print('5. took ' + str((rospy.Time.now() - t).to_sec()))
# idx = np.where(tmpmask == 100)
# # Create tuple with (l, c)
# pointcoords = list(zip(idx[0], idx[1]))
#
# points = pc2.read_points_list(self.msg, skip_nans=False, field_names=("x", "y", "z"))
# tmppoints = []
#
# for coord in pointcoords:
# pointidx = (coord[0]) * 640 + (coord[1])
# tmppoints.append(points[pointidx])
#
# msg_out = createRosCloud(tmppoints, self.msg.header.stamp, self.msg.header.frame_id)
#
# self.publisher_selected_points.publish(msg_out)
print('all. took ' + str((rospy.Time.now() - tall).to_sec()))
elif self.msg_type_str == 'PointCloud2': # 3D scan pointcloud (Andre Aguiar) ---------------------------------
# Get the marker position (this comes from the shpere in rviz)
x_marker, y_marker, z_marker = self.marker.pose.position.x, self.marker.pose.position.y, \
self.marker.pose.position.z # interactive marker pose
# Extract 3D point from the LiDAR
pc = ros_numpy.numpify(self.msg)
points = np.zeros((pc.shape[0], 3))
points[:, 0] = pc['x']
points[:, 1] = pc['y']
points[:, 2] = pc['z']
# Extract the points close to the seed point from the entire PCL
marker_point = np.array([[x_marker, y_marker, z_marker]])
dist = scipy.spatial.distance.cdist(marker_point,
points,
metric='euclidean')
pts = points[np.transpose(dist < self.tracker_threshold)[:, 0], :]
idx = np.where(np.transpose(dist < self.tracker_threshold)[:,
0])[0]
# Tracker - update seed point with the average of cluster to use in the next
# iteration
seed_point = []
if 0 < len(pts):
x_sum, y_sum, z_sum = 0, 0, 0
for i in range(0, len(pts)):
x_sum += pts[i, 0]
y_sum += pts[i, 1]
z_sum += pts[i, 2]
seed_point.append(x_sum / len(pts))
seed_point.append(y_sum / len(pts))
seed_point.append(z_sum / len(pts))
# RANSAC - eliminate the tracker outliers
number_points = pts.shape[0]
if number_points == 0:
return []
# RANSAC iterations
for i in range(0, self.number_iterations):
# Randomly select three points that cannot be coincident
# nor collinear
while True:
idx1 = random.randint(0, number_points - 1)
idx2 = random.randint(0, number_points - 1)
idx3 = random.randint(0, number_points - 1)
pt1, pt2, pt3 = pts[[idx1, idx2, idx3], :]
# Compute the norm of position vectors
ab = np.linalg.norm(pt2 - pt1)
bc = np.linalg.norm(pt3 - pt2)
ac = np.linalg.norm(pt3 - pt1)
# Check if points are colinear
if (ab + bc) == ac:
continue
# Check if are coincident
if idx2 == idx1:
continue
if idx3 == idx1 or idx3 == idx2:
continue
# If all the conditions are satisfied, we can end the loop
break
# ABC Hessian coefficients and given by the external product between two vectors lying on hte plane
A, B, C = np.cross(pt2 - pt1, pt3 - pt1)
# Hessian parameter D is computed using one point that lies on the plane
D = -(A * pt1[0] + B * pt1[1] + C * pt1[2])
# Compute the distance from all points to the plane
# from https://www.geeksforgeeks.org/distance-between-a-point-and-a-plane-in-3-d/
distances = abs(
(A * pts[:, 0] + B * pts[:, 1] + C * pts[:, 2] +
D)) / (math.sqrt(A * A + B * B + C * C))
# Compute number of inliers for this plane hypothesis.
# Inliers are points which have distance to the plane less than a tracker_threshold
num_inliers = (distances < self.ransac_threshold).sum()
# Store this as the best hypothesis if the number of inliers is larger than the previous max
if num_inliers > self.n_inliers:
self.n_inliers = num_inliers
self.A = A
self.B = B
self.C = C
self.D = D
# Extract the inliers
distances = abs((self.A * pts[:, 0] + self.B * pts[:, 1] + self.C * pts[:, 2] + self.D)) / \
(math.sqrt(self.A * self.A + self.B * self.B + self.C * self.C))
inliers = pts[np.where(distances < self.ransac_threshold)]
# Create dictionary [pcl point index, distance to plane] to select the pcl indexes of the inliers
idx_map = dict(zip(idx, distances))
final_idx = []
for key in idx_map:
if idx_map[key] < self.ransac_threshold:
final_idx.append(key)
# -------------------------------------- End of RANSAC ----------------------------------------- #
# publish the points that belong to the cluster
points = []
for i in range(len(inliers)):
r = int(1 * 255.0)
g = int(1 * 255.0)
b = int(1 * 255.0)
a = 150
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [inliers[i, 0], inliers[i, 1], inliers[i, 2], rgb]
points.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgba', 12, PointField.UINT32, 1)
]
header = Header()
header.frame_id = self.parent
header.stamp = self.msg.header.stamp
pc_msg = point_cloud2.create_cloud(header, fields, points)
self.publisher_selected_points.publish(pc_msg)
# Reset the number of inliers to have a fresh start on the next interation
self.n_inliers = 0
# Update the dictionary with the labels (to be saved if the user selects the option)
self.labels['detected'] = True
self.labels['idxs'] = final_idx
# Update the interactive marker pose
self.marker.pose.position.x = seed_point[0]
self.marker.pose.position.y = seed_point[1]
self.marker.pose.position.z = seed_point[2]
self.menu_handler.reApply(self.server)
self.server.applyChanges()
def markerFeedback(self, feedback):
# print(' sensor ' + self.name + ' received feedback')
# pass
# self.optT.setTranslationFromPosePosition(feedback.pose.position)
# self.optT.setQuaternionFromPoseQuaternion(feedback.pose.orientation)
# self.menu_handler.reApply(self.server)
self.server.applyChanges()
def createInteractiveMarker(self):
self.marker = InteractiveMarker()
self.marker.header.frame_id = self.parent
self.marker.pose.position.x = 0
self.marker.pose.position.y = 0
self.marker.pose.position.z = 0
self.marker.pose.orientation.x = 0
self.marker.pose.orientation.y = 0
self.marker.pose.orientation.z = 0
self.marker.pose.orientation.w = 1
self.marker.scale = self.marker_scale
self.marker.name = self.name
self.marker.description = self.name + '_labeler'
# insert a box
control = InteractiveMarkerControl()
control.always_visible = True
marker_box = Marker()
marker_box.type = Marker.SPHERE
marker_box.scale.x = self.marker.scale * 0.3
marker_box.scale.y = self.marker.scale * 0.3
marker_box.scale.z = self.marker.scale * 0.3
marker_box.color.r = 0
marker_box.color.g = 1
marker_box.color.b = 0
marker_box.color.a = 0.2
control.markers.append(marker_box)
self.marker.controls.append(control)
self.marker.controls[
0].interaction_mode = InteractiveMarkerControl.NONE
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
# control = InteractiveMarkerControl()
# control.orientation.w = 1
# control.orientation.x = 0
# control.orientation.y = 1
# control.orientation.z = 0
# control.name = "move_z"
# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
# control.orientation_mode = InteractiveMarkerControl.FIXED
# self.marker.controls.append(control)
# #
# control = InteractiveMarkerControl()
# control.orientation.w = 1
# control.orientation.x = 0
# control.orientation.y = 0
# control.orientation.z = 1
# control.name = "move_y"
# control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
# control.orientation_mode = InteractiveMarkerControl.FIXED
# self.marker.controls.append(control)
self.server.insert(self.marker, self.markerFeedback)
self.menu_handler.apply(self.server, self.marker.name)
def createInteractiveMarkerRGBD(self, x=0, y=0, z=0):
self.marker = InteractiveMarker()
self.marker.header.frame_id = self.parent
self.marker.pose.position.x = x
self.marker.pose.position.y = y
self.marker.pose.position.z = z
self.marker.pose.orientation.x = 0
self.marker.pose.orientation.y = 0
self.marker.pose.orientation.z = 0
self.marker.pose.orientation.w = 1
self.marker.scale = self.marker_scale
self.marker.name = self.name
self.marker.description = self.name + '_labeler'
# insert a box
control = InteractiveMarkerControl()
control.always_visible = True
marker_box = Marker()
marker_box.type = Marker.SPHERE
marker_box.scale.x = self.marker.scale * 0.3
marker_box.scale.y = self.marker.scale * 0.3
marker_box.scale.z = self.marker.scale * 0.3
marker_box.color.r = 1
marker_box.color.g = 0
marker_box.color.b = 0
marker_box.color.a = 0.2
control.markers.append(marker_box)
self.marker.controls.append(control)
self.marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
self.server.insert(self.marker, self.markerFeedback)
self.menu_handler.apply(self.server, self.marker.name)
infos = bag.read_messages(topics='/camera/rgb/camera_info')
info = infos.next()[1]
model = PinholeCameraModel()
model.fromCameraInfo(info)
paths = [path_root + 'cameron-00.pickle',
path_root + 'matt-00.pickle',
path_root + 'jasper-00.pickle']
for path, color, name in zip(paths, 'kbg', ['Cameron', 'Matthew', 'Jasper']):
with open(path, 'r') as f:
data = pickle.load(f)
depths = [d[0] for d in data]
sizes_pixel = [d[1][-1] for d in data]
top_lefts = [model.projectPixelTo3dRay((u,v))
for (d, (u,v,w,h)) in [(d[0], d[1]) for d in data]]
top_rights = [model.projectPixelTo3dRay((u+w,v))
for (d, (u,v,w,h)) in [(d[0], d[1]) for d in data]]
sizes_meter = [tr[0] / tr[2] * d - tl[0] / tl[2] * d
for tl, tr, d in zip(top_lefts, top_rights, depths)]
size_avg = sum(sizes_meter) / len(sizes_meter)
print size_avg
depths_true = [i*0.01 for i in range(50,501)]
sizes_pixel_true = [model.project3dToPixel((size_avg, 0, d))[0]-
model.project3dToPixel((0, 0, d))[0]
for d in depths_true]
pyplot.plot(sizes_pixel, depths, color+'o', label=name+' Actual')
pyplot.plot(sizes_pixel_true, depths_true, color, label=name+' Calculated, Size = '+str(int(size_avg*1000))+'mm')
class ros_cv_testing_node:
def __init__(self):
# Get information for this particular camera
camera_info = get_single('head_camera/depth_registered/camera_info', CameraInfo)
print camera_info
# Set information for the camera
self.cam_model = PinholeCameraModel()
self.cam_model.fromCameraInfo(camera_info)
# Subscribe to the camera's image topic and depth image topic
self.rgb_image_sub = rospy.Subscriber("head_camera/rgb/image_raw",
Image, self.on_rgb)
self.depth_image_sub = rospy.Subscriber("head_camera/depth_registered/image_raw",
Image, self.on_depth)
# Publish where the button is in the base link frame
self.point_pub = rospy.Publisher('button_point', PointStamped, queue_size=10)
# Set up connection to CvBridge
self.bridge = CvBridge()
# Open up viewing windows
cv2.namedWindow('depth')
cv2.namedWindow('rgb')
# Set up the class variables
self.rgb_image = None
self.rgb_image_time = None
self.depth_image = None
self.center = None
self.return_point = PointStamped()
self.tf_listener = TransformListener()
def on_rgb(self, image):
# Convert image to something that cv2 can work with
try:
self.rgb_image = self.bridge.imgmsg_to_cv2(image, 'bgr8')
except e:
print e
return
self.rgb_image_time = image.header.stamp
# Get height and width of image
h = self.rgb_image.shape[0]
w = self.rgb_image.shape[1]
# Create empty image
color_dst = numpy.empty((h,w), 'uint8')
# Convert picture to grayscale
cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2GRAY, color_dst)
# Find circles given the camera image
dp = 1.1
minDist = 90
circles = cv2.HoughCircles(color_dst, cv2.cv.CV_HOUGH_GRADIENT, dp, minDist)
# If no circles are detected then exit the program
if circles == None:
print "No circles found using these parameters"
sys.exit()
circles = numpy.uint16(numpy.around(circles))
# Draw the center of the circle closest to the top
ycoord = []
for i in circles[0,:]:
ycoord.append(i[1])
# Find the top circle in the frame
top_circ_y_coor = min(ycoord)
x_coor = 0
y_coor = 0
for i in circles[0,:]:
if i[1] == top_circ_y_coor:
# draw the center of the circle
#cv2.circle(self.rgb_image,(i[0],i[1]),2,(0,0,255),3)
# draw outer circle
#cv2.circle(self.rgb_image,(i[0],i[1]),i[2],(0,255,0),2)
x_coor = i[0]
y_coor = i[1]
cv2.circle(self.rgb_image,(327,247),2,(0,0,255),3)
# Set the coordinates for the center of the circle
self.center = (x_coor, y_coor)
#self.center = (328,248)
cv2.imshow('rgb', self.rgb_image)
cv2.waitKey(1)
def on_depth(self, image):
# Use numpy so that cv2 can use image
self.depth_image = numpy.fromstring(image.data, dtype='float32').reshape((480,640))
nmask = numpy.isnan(self.depth_image)
#Find the minimum and the maximum of the depth image
Dmin = self.depth_image[~nmask].min()
Dmax = self.depth_image[~nmask].max()
# If a circle has been found find its depth and apply that to the ray
if self.center!=None:
ray = self.cam_model.projectPixelTo3dRay(self.center)
depth = self.depth_image[self.center[1]][self.center[0]]
# If the depth is not a number exit
if math.isnan(depth):
print "Incomplete information on depth at this point"
return
# Otherwise mulitply the depth by the unit vector of the ray
else:
print "depth", depth
cam_coor = [depth*ray[0],depth*ray[1],depth*ray[2]]
#print "camera frame coordinate:", cam_coor
else:
return
# Rescale to [0,1]
cv2.imshow('depth', self.depth_image / 2.0)
cv2.waitKey(1)
# Only use depth if the RGB image is running
if self.rgb_image_time is None:
rospy.loginfo('not using depth cause no RGB yet')
return
time_since_rgb = image.header.stamp - self.rgb_image_time
# Only use depth if it is close in time to the RGB image
if time_since_rgb > rospy.Duration(0.5):
rospy.loginfo('not using depth because time since RGB is ' + str(time_since_rgb))
#return
# Find transform from camera frame to world frame
self.tf_listener.waitForTransform(self.cam_model.tfFrame(), "base_link",
image.header.stamp, rospy.Duration(1.0))
# Set up the point to be published
self.return_point.header.stamp = image.header.stamp
self.return_point.header.frame_id = self.cam_model.tfFrame()
self.return_point.point.x = cam_coor[0]
self.return_point.point.y = cam_coor[1]
self.return_point.point.z = cam_coor[2]
# Transform point to the baselink frame
self.return_point = self.tf_listener.transformPoint("base_link", self.return_point)
print "world frame coordinate:", self.return_point.point.x, \
self.return_point.point.y,self.return_point.point.z, "\n"
self.point_pub.publish(self.return_point)
class MarkerOccGrid(OccGridUtils):
'''
Handles updating occupancy grid when new data comes in.
TODO: Upon call can return some path to go to in order to find them.
'''
def __init__(self, image_sub, grid_res, grid_width, grid_height, grid_starting_pose):
super(self.__class__, self).__init__(res=grid_res, width=grid_width, height=grid_height, starting_pose=grid_starting_pose)
self.tf_listener = tf.TransformListener()
self.cam = PinholeCameraModel()
self.camera_info = image_sub.wait_for_camera_info()
if self.camera_info is None:
# Maybe raise an alarm here.
rospy.logerr("I don't know what to do without my camera info.")
self.cam.fromCameraInfo(self.camera_info)
def update_grid(self, timestamp):
'''
Takes marker information to update occupacy grid.
'''
x_y_position, height = self.get_tf(timestamp)
self.add_circle(x_y_position, self.calculate_visual_radius(height))
self.publish_grid()
def add_marker(self, marker, timestamp):
'''
Find the actual 3d pose of the marker and fill in the occupancy grid for that pose.
This works by:
1. Calculate unit vector between marker point and the image center in the image frame.
2. Use height measurement to find real life distance (m) between center point and marker center.
3. Use unit vec and magnitude to find dx and dy in meters.
3. Pass info to OccGridUtils.
'''
if marker[0] is None:
return
x_y_position, height = self.get_tf(timestamp)
if marker[2] < self.calculate_marker_area(height) * .6:
# If the detected region isn't big enough dont add it.
rospy.logwarn("Marker found but it is too small, not adding marker.")
return
# Calculate position of marker accounting for camera rotation.
dir_vector = unit_vector(np.array([self.cam.cx(), self.cam.cy()] - marker[0]))
trans, rot = self.tf_listener.lookupTransform("/map", "/downward", timestamp)
cam_rotation = tf.transformations.euler_from_quaternion(rot)[2] + np.pi / 2
dir_vector = np.dot(dir_vector, make_2D_rotation(cam_rotation))
marker_rotation = cam_rotation + marker[1]
trans, rot = self.tf_listener.lookupTransform("/map", "/base_link", timestamp)
if (np.abs(np.array(rot)[:2]) > .005).any():
rospy.logwarn("We're at a weird angle, not adding marker.")
magnitude = self.calculate_visual_radius(height, second_point=marker[0])
local_position = dir_vector[::-1] * magnitude
position = local_position + x_y_position
#print local_position
# Pose on ground plane from center
pose = Pose2D(x=position[0], y=position[1], theta=marker_rotation)
self.found_marker(pose)
# The image coordinate pose and real life pose are different.
pose = Pose2D(x=position[0], y=position[1], theta=marker_rotation)
# In meters with initial point at (0,0)
return pose
def get_tf(self, timestamp=None):
'''
x_y position and height in meters
'''
if timestamp is None:
timestamp = rospy.Time()
self.tf_listener.waitForTransform("/map", "/downward", timestamp, rospy.Duration(1.0))
trans, rot = self.tf_listener.lookupTransform("/map", "/downward", timestamp)
x_y_position = trans[:2]
self.tf_listener.waitForTransform("/ground", "/downward", timestamp, rospy.Duration(1.0))
trans, _ = self.tf_listener.lookupTransform("/ground", "/downward", timestamp)
height = np.nan_to_num(trans[2])
return x_y_position, height
def correct_height(self, measured_height, timestamp):
'''
Adjust the measured height from the seafloor using our orientation relative to it.
We assume the floor is flat (should be valid for transdec but maybe not for pool).
All the math is just solving triangles.
Note: If the roll or pitch is too far off, the range could be to a point that is not
planar to the floor directly under us - in which case this will fail.
TODO: See if this actually can produce better results.
'''
trans, rot = self.tf_listener.lookupTransform("/map", "/base_link", timestamp)
euler_rotation = tf.transformations.euler_from_quaternion(rot)
roll_offset = np.abs(np.sin(euler_rotation[0]) * measured_height)
pitch_offset = np.abs(np.sin(euler_rotation[1]) * measured_height)
height = np.sqrt(measured_height ** 2 - (roll_offset ** 2 + pitch_offset ** 2))
return height
def calculate_marker_area(self, height):
'''
Esitmate what the area of the marker should be so we don't add incorrect markers to the occupancy grid.
What we really don't want is to add markers to the grid that are on the edge since the direction and center of
the marker are off.
'''
MARKER_LENGTH = 1.22 # m
MARKER_WIDTH = .1524 # m
# Get m/px on the ground floor.
m = self.calculate_visual_radius(height)
if self.cam.cy() < self.cam.cx():
px = self.cam.cy()
else:
px = self.cam.cx()
m_px = m / px
marker_area_m = MARKER_WIDTH * MARKER_LENGTH
marker_area_px = marker_area_m / (m_px ** 2)
return marker_area_px
def calculate_visual_radius(self, height, second_point=None):
'''
Draws rays to find the radius of the FOV of the camera in meters.
It also can work to find the distance between two planar points some distance from the camera.
'''
mid_ray = np.array([0, 0, 1])
if second_point is None:
if self.cam.cy() < self.cam.cx():
second_point = np.array([self.cam.cx(), 0])
else:
second_point = np.array([0, self.cam.cy()])
edge_ray = unit_vector(self.cam.projectPixelTo3dRay(second_point))
# Calculate angle between vectors and use that to find r
theta = np.arccos(np.dot(mid_ray, edge_ray))
return np.tan(theta) * height
class image_conversion:
def __init__(self):
self.bridge = CvBridge()
self.point_pub = rospy.Publisher('weed_pointcloud', PointCloud, queue_size=5)
self.image_sub = rospy.Subscriber("/thorvald_001/kinect2_camera/hd/image_color_rect", Image, self.callback)
self.image_info = rospy.Subscriber("/thorvald_001/kinect2_camera/hd/camera_info", CameraInfo, self.cam_info_callback)
self.cam = None
self.point_msg = PointCloud()
self.tflistener = tf.listener.TransformListener()
def cam_info_callback(self, data):
if self.cam is None:
print('initialise caminfo')
self.cam = PinholeCameraModel()
self.cam.fromCameraInfo(data)
def callback(self, data):
pts = []
image = self.bridge.imgmsg_to_cv2(data, "bgr8")
if self.inputimage == 'simple':
hsv= cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
blur = cv2.GaussianBlur(hsv, ksize=(17, 17), sigmaX=10)
up_w = np.array([40,0,0])
lw_w = np.array([180,120,255])
mask = cv2.inRange(blur, up_w, lw_w)
res = cv2.bitwise_and(image, image, mask=mask)
gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
ret, thresh = cv2.threshold(gray, 10, 255, 0)
cv2.imshow('thresh', thresh)
result = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
print("Number of contours=" + str(len(result)))
if len(result)==2:
contours, hierarchy = result
elif len(result)==3:
im2, contours, hierarchy = result
threshold_area = 500
filtered_contours = []
for count in contours:
area = cv2.contourArea(count)
print(area)
if area > threshold_area:
filtered_contours.append(count)
cv2.drawContours(image, filtered_contours, -1, (0, 255, 0), 3)
rects=[]
for count in filtered_contours:
x, y, w, h = cv2.boundingRect(count)
rects.append([x, y, w, h])
rects.append([x, y, w, h])
cv2.rectangle(image,(x,y),(x+w,y+h),(0,255,0),8)
contours_points=[]
contours_boxes, weights = cv2.groupRectangles(rects, 1, 0.2)
for rect in contours_boxes:
middle = (x + w / 2, y + h / 2)
if self.cam:
xa, ya, za = self.cam.projectPixelTo3dRay(middle)
print (xa, ya, za)
pts.append(Point32(xa/2, ya/2, za/2))
contours_points.append(middle)
cv2.circle(image, middle, 7, (255, 255, 255), -1)
x, y, w, h = rect
cv2.rectangle(image, (x, y), (x + w, y + h), (255, 0, 0), 4)
cv2.imshow('image', image)
cv2.imshow('mask', mask)
cv2.waitKey(1)
time = rospy.Time(0)
self.point_msg.points = pts
# self.point_msg.header.frame_id = self.cam.tfFrame()
self.point_msg.header.frame_id = 'map'
self.point_msg.header.stamp = time
# tf_points = self.tflistener.transformPointCloud('map', self.points_msg)
# self.point_pub.publish(tf_points)
self.point_pub.publish(self.point_msg)
#cv2.startWindowThread()
#rospy.init_node('image_conv')
#ic = image_conv()
#rospy.spin()
if __name__=="__main__":
rospy.init_node('image_conv', anonymous=True)
conv = image_conversion()
rospy.spin()
cv2.destroyAllWindows()
class MarkerOccGrid(OccGridUtils):
'''
Handles updating occupancy grid when new data comes in.
TODO: Upon call can return some path to go to in order to find them.
'''
def __init__(self, image_sub, grid_res, grid_width, grid_height,
grid_starting_pose):
super(self.__class__, self).__init__(res=grid_res,
width=grid_width,
height=grid_height,
starting_pose=grid_starting_pose)
self.tf_listener = tf.TransformListener()
self.cam = PinholeCameraModel()
self.camera_info = image_sub.wait_for_camera_info()
if self.camera_info is None:
# Maybe raise an alarm here.
rospy.logerr("I don't know what to do without my camera info.")
self.cam.fromCameraInfo(self.camera_info)
def update_grid(self, timestamp):
'''
Takes marker information to update occupacy grid.
'''
x_y_position, height = self.get_tf(timestamp)
self.add_circle(x_y_position, self.calculate_visual_radius(height))
self.publish_grid()
def add_marker(self, marker, timestamp):
'''
Find the actual 3d pose of the marker and fill in the occupancy grid for that pose.
This works by:
1. Calculate unit vector between marker point and the image center in the image frame.
2. Use height measurement to find real life distance (m) between center point and marker center.
3. Use unit vec and magnitude to find dx and dy in meters.
3. Pass info to OccGridUtils.
'''
if marker[0] is None:
return
x_y_position, height = self.get_tf(timestamp)
if marker[2] < self.calculate_marker_area(height) * .6:
# If the detected region isn't big enough dont add it.
rospy.logwarn(
"Marker found but it is too small, not adding marker.")
return
# Calculate position of marker accounting for camera rotation.
dir_vector = unit_vector(
np.array([self.cam.cx(), self.cam.cy()] - marker[0]))
trans, rot = self.tf_listener.lookupTransform("/map", "/downward",
timestamp)
cam_rotation = tf.transformations.euler_from_quaternion(
rot)[2] + np.pi / 2
dir_vector = np.dot(dir_vector, make_2D_rotation(cam_rotation))
marker_rotation = cam_rotation + marker[1]
trans, rot = self.tf_listener.lookupTransform("/map", "/base_link",
timestamp)
if (np.abs(np.array(rot)[:2]) > .005).any():
rospy.logwarn("We're at a weird angle, not adding marker.")
magnitude = self.calculate_visual_radius(height,
second_point=marker[0])
local_position = dir_vector[::-1] * magnitude
position = local_position + x_y_position
#print local_position
# Pose on ground plane from center
pose = Pose2D(x=position[0], y=position[1], theta=marker_rotation)
self.found_marker(pose)
# The image coordinate pose and real life pose are different.
pose = Pose2D(x=position[0], y=position[1], theta=marker_rotation)
# In meters with initial point at (0,0)
return pose
def get_tf(self, timestamp=None):
'''
x_y position and height in meters
'''
if timestamp is None:
timestamp = rospy.Time()
self.tf_listener.waitForTransform("/map", "/downward", timestamp,
rospy.Duration(1.0))
trans, rot = self.tf_listener.lookupTransform("/map", "/downward",
timestamp)
x_y_position = trans[:2]
self.tf_listener.waitForTransform("/ground", "/downward", timestamp,
rospy.Duration(1.0))
trans, _ = self.tf_listener.lookupTransform("/ground", "/downward",
timestamp)
height = np.nan_to_num(trans[2])
return x_y_position, height
def correct_height(self, measured_height, timestamp):
'''
Adjust the measured height from the seafloor using our orientation relative to it.
We assume the floor is flat (should be valid for transdec but maybe not for pool).
All the math is just solving triangles.
Note: If the roll or pitch is too far off, the range could be to a point that is not
planar to the floor directly under us - in which case this will fail.
TODO: See if this actually can produce better results.
'''
trans, rot = self.tf_listener.lookupTransform("/map", "/base_link",
timestamp)
euler_rotation = tf.transformations.euler_from_quaternion(rot)
roll_offset = np.abs(np.sin(euler_rotation[0]) * measured_height)
pitch_offset = np.abs(np.sin(euler_rotation[1]) * measured_height)
height = np.sqrt(measured_height**2 -
(roll_offset**2 + pitch_offset**2))
return height
def calculate_marker_area(self, height):
'''
Esitmate what the area of the marker should be so we don't add incorrect markers to the occupancy grid.
What we really don't want is to add markers to the grid that are on the edge since the direction and center of
the marker are off.
'''
MARKER_LENGTH = 1.22 # m
MARKER_WIDTH = .1524 # m
# Get m/px on the ground floor.
m = self.calculate_visual_radius(height)
if self.cam.cy() < self.cam.cx():
px = self.cam.cy()
else:
px = self.cam.cx()
m_px = m / px
marker_area_m = MARKER_WIDTH * MARKER_LENGTH
marker_area_px = marker_area_m / (m_px**2)
return marker_area_px
def calculate_visual_radius(self, height, second_point=None):
'''
Draws rays to find the radius of the FOV of the camera in meters.
It also can work to find the distance between two planar points some distance from the camera.
'''
mid_ray = np.array([0, 0, 1])
if second_point is None:
if self.cam.cy() < self.cam.cx():
second_point = np.array([self.cam.cx(), 0])
else:
second_point = np.array([0, self.cam.cy()])
edge_ray = unit_vector(self.cam.projectPixelTo3dRay(second_point))
# Calculate angle between vectors and use that to find r
theta = np.arccos(np.dot(mid_ray, edge_ray))
return np.tan(theta) * height
class Lego_Gripper(object):
def __init__(self, graspPlanner, cam, options, gripper):
#topic_name = '/hsrb/head_rgbd_sensor/depth_registered/image_raw'
not_read = True
while not_read:
try:
cam_info = cam.read_info_data()
if (not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
self.pcm.fromCameraInfo(cam_info)
self.br = tf.TransformBroadcaster()
self.tl = tf.TransformListener()
self.gp = graspPlanner
self.gripper = gripper
self.options = options
self.com = COM()
self.tension = Tensioner()
def compute_trans_to_map(self, norm_pose, rot):
pose = self.tl.lookupTransform('map', 'rgbd_sensor_rgb_frame_map',
rospy.Time(0))
M = tf.transformations.quaternion_matrix(pose[1])
M_t = tf.transformations.translation_matrix(pose[0])
M[:, 3] = M_t[:, 3]
M_g = tf.transformations.quaternion_matrix(rot)
M_g_t = tf.transformations.translation_matrix(norm_pose)
M_g[:, 3] = M_g_t[:, 3]
M_T = np.matmul(M, M_g)
trans = tf.transformations.translation_from_matrix(M_T)
quat = tf.transformations.quaternion_from_matrix(M_T)
return trans, quat
def loop_broadcast(self, norm_pose, rot, count, rot_object):
norm_pose, rot = self.compute_trans_to_map(norm_pose, rot)
while True:
self.br.sendTransform(
(norm_pose[0], norm_pose[1], norm_pose[2]),
#tf.transformations.quaternion_from_euler(ai=0.0,aj=0.0,ak=0.0),
rot,
rospy.Time.now(),
'bed_i_' + str(count),
#'head_rgbd_sensor_link')
'map')
if rot_object:
self.br.sendTransform(
(0.0, 0.0, -cfg.GRIPPER_HEIGHT),
tf.transformations.quaternion_from_euler(ai=0.0,
aj=0.0,
ak=1.57),
rospy.Time.now(),
'bed_' + str(count),
#'head_rgbd_sensor_link')
'bed_i_' + str(count))
else:
self.br.sendTransform(
(0.0, 0.0, -cfg.GRIPPER_HEIGHT),
tf.transformations.quaternion_from_euler(ai=0.0,
aj=0.0,
ak=0.0),
rospy.Time.now(),
'bed_' + str(count),
#'head_rgbd_sensor_link')
'bed_i_' + str(count))
def broadcast_poses(self, poses, g_count):
#while True:
count = 0
for pose in poses:
num_pose = pose[1]
label = pose[0]
td_points = self.pcm.projectPixelTo3dRay(
(num_pose[0], num_pose[1]))
print "DE PROJECTED POINTS ", td_points
norm_pose = np.array(td_points)
norm_pose = norm_pose / norm_pose[2]
norm_pose = norm_pose * (cfg.MM_TO_M * num_pose[2])
print "NORMALIZED POINTS ", norm_pose
#pose = np.array([td_points[0],td_points[1],0.001*num_pose[2]])
a = tf.transformations.quaternion_from_euler(ai=-2.355,
aj=-3.14,
ak=0.0)
b = tf.transformations.quaternion_from_euler(ai=0.0,
aj=0.0,
ak=1.57)
c = tf.transformations.quaternion_multiply(a, b)
thread.start_new_thread(self.loop_broadcast,
(norm_pose, c, g_count, label))
time.sleep(0.3)
count += 1
def convert_crop(self, pose):
pose[0] = self.options.OFFSET_Y + pose[0]
pose[1] = self.options.OFFSET_X + pose[1]
return pose
def plot_on_true(self, pose, true_img):
#pose = self.convert_crop(pose)
dp = DrawPrediction()
image = dp.draw_prediction(np.copy(true_img), pose)
cv2.imshow('label_given', image)
cv2.waitKey(30)
def find_pick_region_net(self, pose, c_img, d_img, count):
'''
Evaluates the current policy and then executes the motion
specified in the the common class
'''
poses = []
#IPython.embed()
p_list = []
y, x = pose
#Crop D+img
print "PREDICTION ", pose
self.plot_on_true([x, y], c_img)
d_img_c = d_img[y - cfg.BOX:y + cfg.BOX, x - cfg.BOX:cfg.BOX + x]
depth = self.gp.find_mean_depth(d_img_c)
poses.append([1.0, [x, y, depth]])
self.broadcast_poses(poses, count)
def find_pick_region_cc(self, pose, rot, c_img, d_img, count):
'''
Evaluates the current policy and then executes the motion
specified in the the common class
'''
poses = []
#IPython.embed()
p_list = []
y, x = pose
self.plot_on_true([x, y], c_img)
#Crop D+img
d_img_c = d_img[y - cfg.BOX:y + cfg.BOX, x - cfg.BOX:cfg.BOX + x]
depth = self.gp.find_mean_depth(d_img_c)
poses.append([rot, [x, y, depth]])
self.broadcast_poses(poses, count)
def find_pick_region_labeler(self, results, c_img, d_img, count):
'''
Evaluates the current policy and then executes the motion
specified in the the common class
'''
poses = []
#IPython.embed()
p_list = []
for result in results['objects']:
print result
x_min = float(result['box'][0])
y_min = float(result['box'][1])
x_max = float(result['box'][2])
y_max = float(result['box'][3])
x = (x_max - x_min) / 2.0 + x_min
y = (y_max - y_min) / 2.0 + y_min
if cfg.USE_DART:
pose = np.array([x, y])
action_rand = mvn(pose, cfg.DART_MAT)
print "ACTION RAND ", action_rand
print "POSE ", pose
x = action_rand[0]
y = action_rand[1]
self.plot_on_true([x, y], c_img)
#Crop D+img
d_img_c = d_img[y - cfg.BOX:y + cfg.BOX, x - cfg.BOX:cfg.BOX + x]
depth = self.gp.find_mean_depth(d_img_c)
poses.append([result['class'], [x, y, depth]])
self.broadcast_poses(poses, count)
def execute_grasp(self, cards, whole_body, direction):
whole_body.end_effector_frame = 'hand_palm_link'
nothing = True
#self.whole_body.move_to_neutral()
#whole_body.linear_weight = 99.0
whole_body.move_end_effector_pose(geometry.pose(), cards[0])
self.com.grip_squeeze(self.gripper)
whole_body.move_end_effector_pose(geometry.pose(z=-0.1), 'head_down')
self.com.grip_open(self.gripper)
class image_converter:
def __init__(self):
#self.image_pub = rospy.Publisher("image_topic_2",Image)
self.bridge = CvBridge()
self.desiredPixel = (None, 0)
#Adi: Use this topic for Sawyer right hand camera feed
#self.image_sub = rospy.Subscriber("/io/internal_camera/right_hand_camera/image_raw",Image,self.callback)
#Adi: Use this topic for realsense color
#self.image_sub = rospy.Subscriber("/camera/color/image_raw",Image,self.callback)
#Adi: Use this topic for realsense depth
#self.image_sub = rospy.Subscriber("/camera/depth/image_rect_raw", Image,self.callback)
#Adi: Use this topic for Baxter left_hand_camera
self.image_sub = rospy.Subscriber("/cameras/left_hand_camera/image",
Image, self.callback)
#Adi: Define the camera model
self.cam_info = rospy.wait_for_message(
"/cameras/left_hand_camera/camera_info", CameraInfo, timeout=5)
self.cam_model = PinholeCameraModel()
self.cam_model.fromCameraInfo(self.cam_info)
def resize(self, image, width=None, height=None, inter=cv2.INTER_AREA):
# initialize the dimensions of the image to be resized and
# grab the image size
dim = None
(h, w) = image.shape[:2]
# if both the width and height are None, then return the
# original image
if width is None and height is None:
return image
# check to see if the width is None
if width is None:
# calculate the ratio of the height and construct the
# dimensions
r = height / float(h)
dim = (int(w * r), height)
# otherwise, the height is None
else:
# calculate the ratio of the width and construct the
# dimensions
r = width / float(w)
dim = (width, int(h * r))
# resize the image
resized = cv2.resize(image, dim, interpolation=inter)
# return the resized image
return resized
def grab_contours(self, cnts):
# if the length the contours tuple returned by cv2.findContours
# is '2' then we are using either OpenCV v2.4, v4-beta, or
# v4-official
if len(cnts) == 2:
cnts = cnts[0]
# if the length of the contours tuple is '3' then we are using
# either OpenCV v3, v4-pre, or v4-alpha
elif len(cnts) == 3:
cnts = cnts[1]
# otherwise OpenCV has changed their cv2.findContours return
# signature yet again and I have no idea WTH is going on
else:
raise Exception(
("Contours tuple must have length 2 or 3, "
"otherwise OpenCV changed their cv2.findContours return "
"signature yet again. Refer to OpenCV's documentation "
"in that case"))
# return the actual contours array
return cnts
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
#cv_image = self.bridge.imgmsg_to_cv2(data, "passthrough")
#print(cv_image)
#cv2.imshow("image", cv_image)
except CvBridgeError as e:
print(e)
#(rows,cols, channel) = cv_image.shape
#if cols > 60 and rows > 60 :
# cv2.circle(cv_image, self.getDesiredPixel(), 10, 255)
#cv2.imshow("Image window", cv_image)
#cv2.waitKey(3)
#Tennis Ball:
#greenLower = (0, 0, 250)
#greenUpper = (0, 0, 255)
#Blue Cup:
greenLower = (100, 130, 200)
greenUpper = (109, 180, 280)
pts = deque(maxlen=64)
# resize the frame, blur it, and convert it to the HSV
# color space
#cv_image = self.resize(cv_image, width=600)
blurred = cv2.GaussianBlur(cv_image, (11, 11), 0)
#cv2.imshow("blurred", blurred)
hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
cv2.imshow("hsv", hsv)
# construct a mask for the color "green", then perform
# a series of dilations and erosions to remove any small
# blobs left in the mask
mask = cv2.inRange(hsv, greenLower, greenUpper)
cv2.imshow("mask", mask)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
# find contours in the mask and initialize the current
# (x, y) center of the ball
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = self.grab_contours(cnts)
center = None
# only proceed if at least one contour was found
if len(cnts) > 0:
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
# only proceed if the radius meets a minimum size
if radius > 10:
# draw the circle and centroid on the frame,
# then update the list of tracked points
self.desiredPixel = (center, rospy.Time.now())
cv2.circle(cv_image, (int(x), int(y)), int(radius),
(0, 255, 255), 2)
cv2.circle(cv_image, center, 5, (0, 0, 255), -1)
# update the points queue
pts.appendleft(center)
# loop over the set of tracked points
for i in range(1, len(pts)):
# if either of the tracked points are None, ignore
# them
if pts[i - 1] is None or pts[i] is None:
continue
# otherwise, compute the thickness of the line and
# draw the connecting lines
thickness = int(np.sqrt(args["buffer"] / float(i + 1)) * 2.5)
cv2.line(cv_image, pts[i - 1], pts[i], (0, 0, 255), thickness)
cv2.imshow("Image window", cv_image)
cv2.waitKey(3)
#try:
# self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
#except CvBridgeError as e:
# print(e)
def pixToCam(self, u, v):
cam_coord = self.cam_model.projectPixelTo3dRay((u, v))
return cam_coord
def getDesiredPixel(self):
#return (754, 509)
return self.desiredPixel
def LinePlaneCollision(self,
planeNormal,
planePoint,
rayDirection,
rayPoint,
epsilon=1e-6):
# ndotu = np.dot(planeNormal, rayDirection)
ndotu = planeNormal.dot(rayDirection)
if abs(ndotu) < epsilon:
raise RuntimeError("no intersection or line is within plane")
w = rayPoint - planePoint
si = -planeNormal.dot(w) / ndotu
Psi = w + si * rayDirection + planePoint
return Psi
def rayToWorld(self, transform, ray, ar_marker):
# Transform is ar to camera
# Transform ar_marker's normal z vector, origin into camera frame to get planeNormal, planePoint
# rayDirection = pixToCam, rayPoint = 0,0,0
ps = PoseStamped()
ps.header.stamp = rospy.Time.now()
ps.header.frame_id = ar_marker
ps.pose.position.x = 0
ps.pose.position.y = 0
ps.pose.position.z = 0
ps.pose.orientation.x = 0.0
ps.pose.orientation.y = 0.0
ps.pose.orientation.z = 0.0
ps.pose.orientation.w = 1.0
pose = tf2_geometry_msgs.do_transform_pose(ps, transform)
ps1 = PoseStamped()
ps1.header.stamp = rospy.Time.now()
ps1.header.frame_id = ar_marker
ps1.pose.position.x = 0
ps1.pose.position.y = 0
ps1.pose.position.z = 1
ps1.pose.orientation.x = 0.0
ps1.pose.orientation.y = 0.0
ps1.pose.orientation.z = 0.0
ps1.pose.orientation.w = 1.0
pose1 = tf2_geometry_msgs.do_transform_pose(ps1, transform)
planeNormal = np.array([
pose1.pose.position.x - pose.pose.position.x,
pose1.pose.position.y - pose.pose.position.y,
pose1.pose.position.z - pose.pose.position.z
])
planePoint = np.array([
pose.pose.position.x, pose.pose.position.y,
pose.pose.position.z + 0.125
])
rayDirection = np.array([ray[0], ray[1], ray[2]])
rayPoint = np.array([0, 0, 0])
pt = self.LinePlaneCollision(planeNormal, planePoint, rayDirection,
rayPoint)
ps2 = PoseStamped()
ps2.header.stamp = rospy.Time.now()
ps2.header.frame_id = ar_marker
ps2.pose.position.x = pt[0]
ps2.pose.position.y = pt[1]
ps2.pose.position.z = pt[2]
ps2.pose.orientation.x = 0.0
ps2.pose.orientation.y = 0.0
ps2.pose.orientation.z = 0.0
ps2.pose.orientation.w = 1.0
# pt is in camera coordinates.
return ps2
def camToWorld(self, ar_tag_num):
ar_marker = "ar_marker_" + str(ar_tag_num)
#Create a publisher and a tf buffer, which is primed with a tf listener
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
run_while = True
# Loop until the node is killed with Ctrl-C
while not rospy.is_shutdown() and run_while:
try:
rospy.sleep(1)
run_while = False
trans0 = tfBuffer.lookup_transform(
ar_marker, "reference/left_hand_camera", rospy.Time())
trans1 = tfBuffer.lookup_transform("base", ar_marker,
rospy.Time())
#could be source of error
trans2 = tfBuffer.lookup_transform("reference/base", "base",
rospy.Time())
box = PoseStamped()
box.header.frame_id = ar_marker
box.pose.position.x = 0.0
box.pose.position.y = 0.0
box.pose.position.z = 0.0
box.pose.orientation.x = 0.0
box.pose.orientation.y = 1.0
box.pose.orientation.z = 0.0
box.pose.orientation.w = 1.0
#This is a point in the /reference/left_hand_camera frame
# ps = PoseStamped()
# ps.header.stamp = rospy.Time.now()
# ps.header.frame_id = "reference/left_hand_camera"
# ps.pose.position.x = cam_coord[0]
# ps.pose.position.y = cam_coord[1]
# ps.pose.position.z = cam_coord[2]
# ps.pose.orientation.x = 0.0
# ps.pose.orientation.y = 0.0
# ps.pose.orientation.z = 0.0
# ps.pose.orientation.w = 1.0
pose_base = tf2_geometry_msgs.do_transform_pose(box, trans1)
pose_ref = tf2_geometry_msgs.do_transform_pose(
pose_base, trans2)
# pose_ref_base = tf2_geometry_msgs.do_transform_pose(pose_base, trans2)
#translation = trans1.transform.translation
#rot = trans.transform.rotation
#print("AR TAG FRAME TRANS")
#print(translation)
#print(rot)
# TODO finish calling rayToWorld
# ic = image_converter()
pixel = None
while pixel is None:
#u, v = self.getDesiredPixel()
pixel, time = self.getDesiredPixel()
print(pixel)
u, v = pixel[0], pixel[1]
ray = self.pixToCam(u, v)
trans3 = tfBuffer.lookup_transform(
"reference/left_hand_camera", ar_marker, rospy.Time())
pose_pix = self.rayToWorld(trans3, ray, ar_marker)
pose_yay = tf2_geometry_msgs.do_transform_pose(
pose_pix, trans0)
pose_lmao = tf2_geometry_msgs.do_transform_pose(
pose_yay, trans1)
print("RIP", pose_lmao)
pt_lmao = [
pose_lmao.pose.position.x, pose_lmao.pose.position.y,
pose_lmao.pose.position.z
]
# move(pose_lmao.pose.position.x, pose_lmao.pose.position.y, pose_lmao.pose.position.z)
return pt_lmao, pose_ref
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
run_while = True
print(e)
pass
class ColorTrackerROS(object):
def __init__(self):
"""
Initialize Color Tracking ROS interface.
"""
rospy.init_node('color_tracker')
self.tracker = ColorTracker()
self.cv_image = self.processed_image = None # the latest image from the camera
self.boxes = []
self.bridge = CvBridge() # used to convert ROS messages to OpenCV
self.cameraModel = PinholeCameraModel()
self.tf = TransformListener(cache_time=rospy.Duration.from_sec(20))
# parameters ...
self._gui = bool(rospy.get_param('~gui', default=False)) # set to _gui:=true for debug display
self._rate = float(rospy.get_param('~rate', default=50.0)) # processing rate
self._par = float(rospy.get_param('~plate_area', default=0.0338709))
# publishers ...
self.debug_pub = rospy.Publisher("tracker/debug", PoseWithCovarianceStamped, queue_size=10)
self.roomba_pub = rospy.Publisher("visible_roombas", RoombaList, queue_size = 10)
rospy.loginfo("Initializing Color Tracker")
if self._gui:
cv2.namedWindow('preview_window')
cv2.namedWindow('binary')
# start listening ...
rospy.Subscriber("/ardrone/bottom/image_raw", Image, self.process_image)
rospy.Subscriber("/ardrone/bottom/camera_info", CameraInfo, self.on_camera_info)
def process_image(self, msg):
"""
Process image messages from ROS and stash them in an attribute
called cv_image for subsequent processing
"""
try:
self.cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
# get drone position ...
# proper tf below ... hacking for now
self.tf.waitForTransform('map', msg.header.frame_id,
msg.header.stamp, rospy.Duration(0.1))
pos, _ = self.tf.lookupTransform('map', msg.header.frame_id, msg.header.stamp)
# area scale + tolerance
xy2uv = self.cameraModel.getDeltaU(1.0, pos[2]) * self.cameraModel.getDeltaV(1.0, pos[2])
min_area = 0.75 * self._par * xy2uv
# do image processing here
self.boxes, self.processed_image = self.tracker.find_bounding_boxes(self.cv_image, min_area)
listOfRoombas = []
for box in self.boxes:
center = np.mean(box, axis=0)
heading, covarianceOfHeading = get_heading(box, center, self.processed_image)
ray = self.cameraModel.projectPixelTo3dRay(center)
camera_ray = Vector3Stamped(header=msg.header,
vector=Vector3(*ray))
print('camera_ray', camera_ray)
drone_ray = self.tf.transformVector3('base_link', camera_ray)
#print('drone_ray', drone_ray)
# get drone height for ground-plane projection
#multiplier = -pos[2] / drone_ray.vector.z
multiplier = pos[2] / camera_ray.vector.z
camera_to_roomba = np.array([camera_ray.vector.x, camera_ray.vector.y, camera_ray.vector.z]) * multiplier
quat = quaternion_from_euler(0,0,heading)
pose = Pose(position=Point(*camera_to_roomba), orientation = Quaternion(*quat))
pwcs = PoseWithCovarianceStamped(header=Header(frame_id=msg.header.frame_id, stamp=msg.header.stamp),
pose=PoseWithCovariance(
pose=pose,
covariance=np.diag([.2, .2, 0, 0, 0, covarianceOfHeading]).flatten()
))
rb = Roomba(last_seen=msg.header.stamp, frame_id=msg.header.frame_id, type=Roomba.RED,
visible_location=pwcs)
listOfRoombas.append(rb)
self.debug_pub.publish(pwcs)
self.roomba_pub.publish(header=Header(frame_id=msg.header.frame_id,stamp=msg.header.stamp),data=listOfRoombas)
rospy.loginfo_throttle(1.0, 'Boxes : {}'.format(self.boxes))
# TODO: Do something with the boxes
# TODO: make sure it doesn't get too far behind
# self.binary_image = binary_image
# self.cv_image = cv_image
except CvBridgeError as e:
rospy.loginfo_throttle(0.5, "Error loading image : {}".format(e))
def on_camera_info(self, msg):
self.cameraModel.fromCameraInfo(msg)
def run(self):
""" The main run loop, in this node it doesn't do anything """
r = rospy.Rate(self._rate)
while not rospy.is_shutdown():
# start out not issuing any motor commands
if not self.cv_image is None and self._gui:
try:
cv2.imshow('preview_window', self.cv_image)
cv2.imshow('binary', self.processed_image)
cv2.waitKey(5)
except Exception as e:
pass
r.sleep()
class VGripper(object):
def __init__(self, graspPlanner, cam, options):
#topic_name = '/hsrb/head_rgbd_sensor/depth_registered/image_raw'
suction_action = '/hsrb/suction_control'
self.suction_control_client = actionlib.SimpleActionClient(
suction_action, SuctionControlAction)
try:
if not self.suction_control_client.wait_for_server(
rospy.Duration(_CONNECTION_TIMEOUT)):
raise Exception(_suction_action + 'does not exist')
except Exception as e:
rospy.logerr(e)
sys.exit(1)
not_read = True
while not_read:
try:
cam_info = cam.read_info_data()
if (not cam_info == None):
not_read = False
except:
rospy.logerr('info not recieved')
self.pcm = PCM()
IPython.embed()
self.pcm.fromCameraInfo(cam_info)
self.br = tf.TransformBroadcaster()
self.gp = graspPlanner
self.options = options
def broadcast_poses(self, poses):
#while True:
count = 0
for pose in poses:
num_pose = pose[1]
label = pose[0]
td_points = self.pcm.projectPixelTo3dRay(
(num_pose[0], num_pose[1]))
norm_pose = np.array(td_points)
norm_pose = norm_pose / norm_pose[2]
norm_pose = norm_pose * (0.001 * num_pose[2])
#pose = np.array([td_points[0],td_points[1],0.001*num_pose[2]])
self.br.sendTransform(
(norm_pose[0], norm_pose[1], norm_pose[2]),
#tf.transformations.quaternion_from_euler(ai=0.0,aj=0.0,ak=0.0),
tf.transformations.quaternion_from_euler(ai=-2.355,
aj=-3.14,
ak=0.0),
rospy.Time.now(),
'card_' + str(count),
#'head_rgbd_sensor_link')
'head_rgbd_sensor_rgb_frame')
count += 1
def find_pick_region(self, results, c_img, d_img):
'''
Evaluates the current policy and then executes the motion
specified in the the common class
'''
poses = []
#IPython.embed()
p_list = []
for result in results['objects']:
print result
x_min = float(result['box_index'][0])
y_min = float(result['box_index'][1])
x_max = float(result['box_index'][2])
y_max = float(result['box_index'][3])
x = (x_max - x_min) / 2.0 + x_min
y = (y_max - y_min) / 2.0 + y_min
cv2.imshow('debug', c_img[int(y_min):int(y_max),
int(x_min):int(x_max)])
cv2.waitKey(30)
#IPython.embed()
#Crop D+img
d_img_c = d_img[int(y_min):int(y_max), int(x_min):int(x_max)]
depth = self.gp.find_max_depth(d_img_c)
poses.append([result['num_class_label'], [x, y, depth]])
self.broadcast_poses(poses)
def convert_crop(self, pose):
pose[0] = self.options.OFFSET_Y + pose[0]
pose[1] = self.options.OFFSET_X + pose[1]
return pose
def plot_on_true(self, pose, true_img):
pose = self.convert_crop(pose)
true_img[pose[1] - 5:pose[1] + 5, pose[0] - 5:pose[0] + 5, 0] = 255.0
cv2.imshow('debug_wrap', true_img)
cv2.waitKey(30)
def find_pick_region_net(self, results, c_img, d_img, c_img_true):
'''
Evaluates the current policy and then executes the motion
specified in the the common class
'''
poses = []
#IPython.embed()
p_list = []
for result in results:
print result
x = int(result['box'][0])
y = int(result['box'][1])
w = int(result['box'][2] / 2.0)
h = int(result['box'][3] / 2.0)
self.plot_on_true([x, y], c_img_true)
#Crop D+img
d_img_c = d_img[y - h:y + h, x - w:x + w]
depth = self.gp.find_max_depth(d_img_c)
poses.append([result['class'], self.convert_crop([x, y, depth])])
self.broadcast_poses(poses)
class OffboardControl:
locations = np.matrix([
[-1.8, -1.8, 2, 0, 0, 0, 0],
[-2.6, -1.8, 2, 0, 0, 0, 0],
[-2.6, -2.6, 2, 0, 0, 0, 0],
[-1.8, -2.6, 2, 0, 0, 0, 0],
])
search_loc = [80, -60, 20, 0, 0, 0, 0]
drop_loc = [-75.998839, 425.005299, 30, 0, 0, 0, 0]
def mavrosTopicStringRoot(self, uavID=0):
mav_topic_string = 'uav' + str(uavID) + '/mavros/'
return mav_topic_string
def __init__(self):
self.curr_pose = PoseStamped()
self.curr_pose_rover = PoseStamped()
self.cam_img = PoseStamped()
self.des_pose = PoseStamped()
self.destination_pose = PoseStamped()
self.rover_variable = PositionTarget()
self.pixel_img = Image()
self.camera_image = Image()
self.rover_location_x = 0
self.rover_loaction_y = 0
self.rover_location_z = 0
self.rover_location_x_previous = 0
self.rover_location_y_previous = 0
self.rover_location_z_previous = 0
self.truck_target_x = 0
self.truck_target_y = 0
self.truck_target_z = 0
self.rover_velocity_x = 0
self.rover_velocity_y = 0
self.rover_velocity_z = 0
self.rover_velocity_x_1 = 0
self.rover_velocity_y_1 = 0
self.rover_velocity_z_1 = 0
self.rover_velocity_previous_x = 0
self.rover_velocity_previous_y = 0
self.rover_velocity_previous_z = 0
self.image_target = []
self.depth = Image()
self.KP = .005
self.counter = 0
self.destination_x = 0
self.destination_y = 0
self.destination_z = 0
self.destination_x_previous = 0
self.destination_y_previous = 0
self.destination_z_previous = 0
self.hover_x = 0
self.hover_y = 0
self.hover_z = 0
self.detection_count = 0
self.ray_target = []
self.rel_coordinates = []
self.flag = "False"
self.is_ready_to_fly = False
self.hover_loc = [
self.hover_x, self.hover_y, self.hover_z, 0, 0, 0, 0
] # Hovers 3meter above at this location
self.suv_search_loc = [0, 0, 0, 0, 0, 0, 0]
self.suv_search_location = [0, 0, 0, 0, 0, 0, 0]
self.mode = "PROBE"
#self.phase = "SEARCH"
self.dist_threshold = 0.4
self.waypointIndex = 0
self.sim_ctr = 1
self.arm = False
self.range = 0
self.prev_count = 0
self.red_detection_flag = "detected"
# define ros subscribers and publishers
rospy.init_node('OffboardControl', anonymous=True)
self.pose_sub = rospy.Subscriber('uav1/mavros/local_position/pose',
PoseStamped,
callback=self.pose_callback)
self.pose_sub0 = rospy.Subscriber('uav0/mavros/local_position/pose',
PoseStamped,
callback=self.pose_callback0)
self.vel_sub0 = rospy.Subscriber('uav0/mavros/local_position/odom',
Odometry,
callback=self.velocity_callback0)
self.state_sub = rospy.Subscriber('uav1/mavros/state',
State,
callback=self.state_callback)
self.vel_pub = rospy.Publisher('uav1/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
#self.attach = rospy.Publisher('/attach', String, queue_size=10)
camera_info = rospy.wait_for_message("/uav_camera_down/camera_info",
CameraInfo)
camera_info2 = rospy.wait_for_message(
"/uav_camera_front/rgb/camera_info", CameraInfo)
self.pinhole_camera_model = PinholeCameraModel()
self.pinhole_camera_model_rgb = PinholeCameraModel()
self.pinhole_camera_model.fromCameraInfo(camera_info)
rospy.Subscriber('/darknet_ros/bounding_boxes',
BoundingBoxes,
callback=self.yolo)
rospy.Subscriber('/uav_camera_down/image_raw',
Image,
callback=self.pixel_image)
self.decision = rospy.Subscriber(
'/data', String,
callback=self.set_mode) # doesnot appear immediately
self.sonar = rospy.Subscriber('/sonar',
Range,
callback=self.range_callback)
self.kalman_filter = rospy.Subscriber('/kf_coords',
gps_kf,
callback=self.kf_callback)
NUM_UAV = 2
mode_proxy = [None for i in range(NUM_UAV)]
arm_proxy = [None for i in range(NUM_UAV)]
for uavID in range(0, NUM_UAV):
mode_proxy[uavID] = rospy.ServiceProxy(
self.mavrosTopicStringRoot(uavID) + '/set_mode', SetMode)
arm_proxy[uavID] = rospy.ServiceProxy(
self.mavrosTopicStringRoot(uavID) + '/cmd/arming', CommandBool)
self.controller()
def yolo(self, data):
for a in data.bounding_boxes:
if a.Class == "truck" or a.Class == "bus" or a.Class == "SUV" or a.Class == "tvmonitor" or a.Class == "traffic light": #"kite"
self.detection_count = self.detection_count + 1
#print(self.detection_count)
X = a.xmin + (a.xmax - a.xmin) / 2
Y = a.ymin + (a.ymax - a.ymin) / 2
#print(a.xmin)
#print(X)
self.image_target = list(
self.pinhole_camera_model.projectPixelTo3dRay((X, Y)))
self.image_target[:] = [
x / self.image_target[2] for x in self.image_target
]
height = self.range
self.truck_target_x1 = self.image_target[0] * height
self.truck_target_y1 = self.image_target[1] * height
self.truck_target_z1 = height
relative_coordinates = np.array([[self.truck_target_x1],
[self.truck_target_y1],
[self.truck_target_z1]])
hom_transformation = np.array([[0, 1, 0, 0], [1, 0, 0, 0],
[0, 0, -1, 0], [0, 0, 0, 1]])
homogeneous_coordinates = np.array([[
relative_coordinates[0][0]
], [relative_coordinates[1][0]], [relative_coordinates[2][0]],
[1]])
product = np.matmul(hom_transformation,
homogeneous_coordinates)
self.truck_target_x = -product[0][0]
self.truck_target_y = -product[1][0]
self.truck_target_z = product[2][0]
#print('X_coordinate_truck',self.truck_target_x)
#print('Y_coordinate_truck',self.truck_target_y)
#print('Z_coordinate_truck',self.truck_target_z)
def pixel_image(self, img):
self.camera_image = img
try:
self.pixel_img = CvBridge().imgmsg_to_cv2(
self.camera_image, desired_encoding='passthrough')
except CvBridgeError as e:
print(e)
#image = cv2.cvtColor(self.pixel_img, cv2.COLOR_BGR2RGB)
#clean image
#image_blur = cv2.GaussianBlur(image,(3,3),0)
image_blur = cv2.GaussianBlur(self.pixel_img, (3, 3), 0)
#image_blur_hsv= cv2.cvtColor(image_blur, cv2.COLOR_RGB2HSV)
image_blur_hsv = cv2.cvtColor(image_blur, cv2.COLOR_BGR2HSV)
#filter by colour1
min_red = np.array([120, 40, 80])
max_red = np.array([190, 230, 255])
mask1 = cv2.inRange(image_blur_hsv, min_red, max_red)
#filter by colour2
min_red2 = np.array([200, 40, 80])
max_red2 = np.array([256, 230, 255])
mask2 = cv2.inRange(image_blur_hsv, min_red2, max_red2)
# Use the two masks to create double mask
mask = mask1 + mask2
edged = cv2.Canny(mask, 30, 200)
contours, heirarchy = cv2.findContours(
edged, cv2.RETR_TREE,
cv2.CHAIN_APPROX_NONE)[-2:] # Fix suggested on stack overflow
cv2.drawContours(image_blur_hsv, contours, -1, (0, 255, 0), 3)
cv2.imshow("Image_window", image_blur_hsv)
cv2.waitKey(3)
if np.size(contours) >= 1:
self.flag = "True"
#print np.shape(contours[0])
contour_area = [(cv2.contourArea(c)) for c in contours]
contour_sizes = [(cv2.contourArea(contour), contour)
for contour in contours]
biggest_contour = max(contour_sizes, key=lambda x: x[0])[1]
elif np.size(contours) < 1:
self.flag = "False"
if self.flag == "True":
self.counter = self.counter + 1
#print(self.counter)
alpha1 = 0.1
# Finding the area within the biggest contour and hence finding the centroid:
area = cv2.contourArea(biggest_contour)
M = cv2.moments(biggest_contour)
if M["m00"] == 0:
cx = int(M["m10"] / 0.000001)
cy = int(M["m01"] / 0.000001)
else:
cx = int(M["m10"] / M["m00"])
cy = int(M["m01"] / M["m00"])
actualx = cx #Pixel x value
actualy = cy #Pixel y value
#print('Pixel X:', actualx)
#print('Pixel Y:', actualy)
self.ray_target = list(
self.pinhole_camera_model.projectPixelTo3dRay(
(actualx, actualy)))
self.ray_target[:] = [
x / self.ray_target[2] for x in self.ray_target
] # rescaling the ray_target such that z is 1
#self.ray_target[:] = [x/650 for x in self.ray_target] #159.88
#print(self.ray_target)
height = self.range
x = self.ray_target[0] * height
y = self.ray_target[1] * height
z = height
self.rel_coordinates = np.array(
[[x], [y],
[z]]) # rel_coordinates stand for relative cordinates
#print x
#print y
#print z
hom_transformation = np.array([[0, 1, 0, 0], [1, 0, 0, 0],
[0, 0, -1, 0], [0, 0, 0, 1]])
homogeneous_coordinates = np.array([[self.rel_coordinates[0][0]],
[self.rel_coordinates[1][0]],
[self.rel_coordinates[2][0]],
[1]])
product = np.matmul(hom_transformation, homogeneous_coordinates)
self.cam_img.pose.position.x = -product[0][0]
self.cam_img.pose.position.y = -product[1][0]
self.cam_img.pose.position.z = product[2][0]
#print('X',-product[0][0])
#print('Y',-product[1][0])
#print('Z',-product[2][0])
self.cam_img.pose.orientation.x = 0
self.cam_img.pose.orientation.y = 0
self.cam_img.pose.orientation.z = 0
self.cam_img.pose.orientation.w = 1
self.destination_pose.pose.position.x = self.cam_img.pose.position.x + self.curr_pose.pose.position.x
self.destination_pose.pose.position.y = self.cam_img.pose.position.y + self.curr_pose.pose.position.y
self.destination_pose.pose.position.z = self.cam_img.pose.position.z + self.curr_pose.pose.position.z
self.destination_pose.pose.orientation.x = self.curr_pose.pose.orientation.x
self.destination_pose.pose.orientation.y = self.curr_pose.pose.orientation.y
self.destination_pose.pose.orientation.z = self.curr_pose.pose.orientation.z
self.destination_pose.pose.orientation.w = self.curr_pose.pose.orientation.w
self.destination_x = (
(1 - alpha1) * self.destination_pose.pose.position.x) + (
alpha1 * self.destination_x_previous)
self.destination_y = (
(1 - alpha1) * self.destination_pose.pose.position.y) + (
alpha1 * self.destination_y_previous)
self.destination_z = (
(1 - alpha1) * self.destination_pose.pose.position.z) + (
alpha1 * self.destination_z_previous)
diff_x = self.destination_x - self.destination_x_previous
diff_y = self.destination_y - self.destination_y_previous
diff_z = self.destination_z - self.destination_z_previous
if self.counter == 0 or self.counter % 20 == 0:
self.destination_x_previous = self.destination_x
self.destination_y_previous = self.destination_y
self.destination_z_previous = self.destination_z
if (diff_x <= 0.005 and diff_x >= -0.005) and (
diff_y <= 0.005
and diff_y >= -0.005) and (diff_z <= 0.005
and diff_z >= -0.005):
self.red_detection_flag = "no detection"
else:
self.red_detection_flag = "detected"
def set_mode(self, msg):
#print('set_mode')
self.mode = str(msg.data)
def pose_callback(self, msg):
#print('pose_callback')
self.curr_pose = msg
def pose_callback0(self, msg):
self.curr_pose_rover = msg
self.rover_location_x = self.curr_pose_rover.pose.position.x
self.rover_location_y = self.curr_pose_rover.pose.position.y
self.rover_location_z = self.curr_pose_rover.pose.position.z
self.suv_search_loc = [
self.rover_location_x, self.rover_location_y,
self.rover_location_z, 0, 0, 0, 0
]
if self.prev_count == 0 or self.prev_count % 20 == 0:
self.rover_location_x_previous = self.rover_location_x
self.rover_location_y_previous = self.rover_location_y
self.rover_location_z_previous = self.rover_location_z
self.prev_count += 1
#print("Current", self.rover_location_x)
#print("Past", self.rover_location_x_previous)
def quaternion_to_yaw(self, w, x, y, z):
#"""Converts quaternions with components w, x, y, z into a tuple (roll, pitch, yaw)"""
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y**2 + z**2)
yaw = np.arctan2(siny_cosp, cosy_cosp)
return yaw
def velocity_callback0(self, velocity):
beta = 0.1
self.rover_variable = velocity
#print("The rover variable is :", self.rover_variable )
x_rotation = self.rover_variable.pose.pose.orientation.x
y_rotation = self.rover_variable.pose.pose.orientation.y
z_rotation = self.rover_variable.pose.pose.orientation.z
w_rotation = self.rover_variable.pose.pose.orientation.w
euler_z = self.quaternion_to_yaw(w_rotation, x_rotation, y_rotation,
z_rotation)
#print("The euler rotation Z", euler_z)
z_net_rot = -euler_z
#print("The net z rotation is :", z_net_rot)
cos_angle = math.cos(z_net_rot)
sin_angle = math.sin(z_net_rot)
matrix_1 = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
matrix_2 = np.array([[cos_angle, -sin_angle, 0],
[sin_angle, cos_angle, 0], [0, 0, 1]])
matrix = np.matmul(matrix_1, matrix_2)
hom_transformation = np.array(
[[matrix[0][0], matrix[0][1], matrix[0][2], 0],
[matrix[1][0], matrix[1][1], matrix[1][2], 0],
[matrix[2][0], matrix[2][1], matrix[2][2], 0], [0, 0, 0, 1]])
#hom_transformation = np.array([[cos_angle, -sin_angle, 0, 0],[sin_angle, cos_angle, 0, 0],[0, 0, 1, 0],[0, 0, 0, 1]])
#print("The homogeneous transformation matrix is :", hom_transformation)
self.rover_velocity_x_1 = self.rover_variable.twist.twist.linear.x
self.rover_velocity_y_1 = self.rover_variable.twist.twist.linear.y
self.rover_velocity_z_1 = self.rover_variable.twist.twist.linear.z
if self.rover_location_x - self.rover_location_x_previous < 0.001 and self.rover_location_x - self.rover_location_x_previous > -0.001:
self.rover_velocity_x_1 = 0
if self.rover_location_y - self.rover_location_y_previous < 0.001 and self.rover_location_y - self.rover_location_y_previous > -0.001:
self.rover_velocity_y_1 = 0
if self.rover_location_z - self.rover_location_z_previous < 0.0001 and self.rover_location_z - self.rover_location_z_previous > -0.0001:
self.rover_velocity_z_1 = 0
hom_velocity = np.array([[self.rover_velocity_x_1],
[self.rover_velocity_y_1],
[self.rover_velocity_z_1], [1]])
product = np.matmul(hom_transformation, hom_velocity)
self.rover_velocity_x = ((1 - beta) * product[0][0] +
(beta) * self.rover_velocity_previous_x)
self.rover_velocity_y = ((1 - beta) * product[1][0] +
(beta) * self.rover_velocity_previous_y)
self.rover_velocity_z = ((1 - beta) * product[2][0] +
(beta) * self.rover_velocity_previous_z)
self.rover_velocity_previous_x = self.rover_velocity_x
self.rover_velocity_previous_y = self.rover_velocity_y
self.rover_velocity_previous_z = self.rover_velocity_z
#print("The X velocity of the ROVER",self.rover_velocity_x,"_________________________________________________")
#print("The Y velocity of the ROVER",self.rover_velocity_y,"____________________________________________")
#print("The Z velocity of the ROVER",self.rover_velocity_z,"____________________________________________________________")
def range_callback(self, msg):
#print(msg)
self.range = msg.range
def kf_callback(self, msg):
self.X_x = msg.X_x
self.X_y = msg.X_y
self.X_z = msg.X_z
#print(self.X_x, self.X_y, self.X_z)
def state_callback(self, msg):
#print('state_callback')
if msg.mode == 'OFFBOARD' and self.arm == True:
self.is_ready_to_fly = True
else:
self.take_off()
def set_offboard_mode(self):
#print('set_offboardmode')
rospy.wait_for_service('uav1/mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('uav1/mavros/set_mode',
SetMode)
isModeChanged = flightModeService(custom_mode='OFFBOARD')
except rospy.ServiceException as e:
print(
"service set_mode call failed: %s. OFFBOARD Mode could not be set. Check that GPS is enabled"
% e)
def set_arm(self):
#print('set_arm')
rospy.wait_for_service('uav1/mavros/cmd/arming')
try:
armService = rospy.ServiceProxy('uav1/mavros/cmd/arming',
CommandBool)
armService(True)
self.arm = True
except rospy.ServiceException as e:
print("Service arm call failed: %s" % e)
def take_off(self):
#print('take_off')
self.set_offboard_mode()
self.set_arm()
def attach(self):
print('Running attach_srv')
attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach)
attach_srv.wait_for_service()
print('Trying to attach')
req = AttachRequest()
req.model_name_1 = "iris_1"
req.link_name_1 = "base_link"
req.model_name_2 = "sample_probe"
req.link_name_2 = "base_link"
attach_srv.call(req)
print('Attached')
def detach(self):
attach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach)
attach_srv.wait_for_service()
print("detaching")
req = AttachRequest()
req.model_name_1 = "iris_1"
req.link_name_1 = "base_link"
req.model_name_2 = "sample_probe"
req.link_name_2 = "base_link"
attach_srv.call(req)
def hover(self):
""" hover at height mentioned in location
set mode as HOVER to make it work
"""
print("In hover")
location = self.hover_loc
loc = [
list(location),
list(location),
list(location),
list(location),
list(location)
]
#print(loc)
rate = rospy.Rate(20)
rate.sleep()
shape = len(loc)
pose_pub = rospy.Publisher('uav1/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
self.des_pose = self.copy_pose(self.curr_pose)
waypoint_index = 0
sim_ctr = 1
#print(self.mode)
while self.mode == "HOVER" and self.counter <= 35 and not rospy.is_shutdown(
):
if waypoint_index == 5:
waypoint_index = 0
sim_ctr += 1
#print("HOVER COUNTER: " + str(sim_ctr))
des_x = loc[waypoint_index][0]
des_y = loc[waypoint_index][1]
des_z = loc[waypoint_index][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 = 0
self.des_pose.pose.orientation.y = 0
self.des_pose.pose.orientation.z = 0
self.des_pose.pose.orientation.w = 0
curr_x = self.curr_pose.pose.position.x
curr_y = self.curr_pose.pose.position.y
curr_z = self.curr_pose.pose.position.z
#print([curr_x, curr_y, curr_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))
#print(dist)
if dist < self.dist_threshold:
waypoint_index += 1
pose_pub.publish(self.des_pose)
rate.sleep()
# if self.counter > 35:
# break
def copy_pose(self, pose):
pt = pose.pose.position
quat = pose.pose.orientation
copied_pose = PoseStamped()
copied_pose.header.frame_id = pose.header.frame_id
copied_pose.pose.position = Point(pt.x, pt.y, pt.z)
copied_pose.pose.orientation = Quaternion(quat.x, quat.y, quat.z,
quat.w)
return copied_pose
def get_descent(self, x, y, z):
#print("In get_desce nt")
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 = y #y
des_vel.velocity.y = x #x
des_vel.velocity.z = z
return des_vel
def flytodestination(self, destination):
print("flytodestination")
rate = rospy.Rate(10)
rate.sleep()
waypointidx = 0
#print("Waypointidx",waypointidx)
pose_pub = rospy.Publisher('uav1/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
self.des_pose = self.copy_pose(self.curr_pose)
while not rospy.is_shutdown():
if waypointidx is 1:
break
if self.is_ready_to_fly:
des_x = destination[0]
des_y = destination[1]
des_z = destination[2]
#z_increase = 10 - self.range # The integer value is chose such that the drone maintins the same value in units from the ground
#des_z = self.curr_pose.pose.position.z + z_increase
self.des_pose.pose.position.x = des_x
self.des_pose.pose.position.y = des_y
self.des_pose.pose.position.z = des_z
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.dist_threshold:
waypointidx += 1
print(waypointidx)
pose_pub.publish(self.des_pose)
rate.sleep()
def pattern(self, hover_dist, start_loc, threshold=0.3, second_run=False):
print("PATTERN")
#print(self.mode)
self.sim_ctr = 0
self.counter = 0
rate = rospy.Rate(10) # Hz
rate.sleep()
pose_pub = rospy.Publisher('uav1/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
self.des_pose = self.copy_pose(self.curr_pose)
shape = self.locations.shape
self.detection_count = 0
mower_ctr = 0
x_increase = 0
y_increase = 0
z_increase = 0
#print(start_loc)
des_x = start_loc[0]
des_y = start_loc[1]
z_increase = hover_dist - self.range # The integer value is chose such that the drone maintins the same value in units from the ground
des_z = self.curr_pose.pose.position.z + z_increase
while not rospy.is_shutdown():
if self.waypointIndex is shape[0]:
#print("I am here")
self.waypointIndex = 0
self.sim_ctr += 1
if self.is_ready_to_fly:
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 = 0
self.des_pose.pose.orientation.y = 0
self.des_pose.pose.orientation.z = 0
self.des_pose.pose.orientation.w = 0
curr_x = self.curr_pose.pose.position.x
curr_y = self.curr_pose.pose.position.y
curr_z = self.curr_pose.pose.position.z
x_increase = 0
y_increase = 0
z_increase = 0
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 < threshold:
self.waypointIndex += 1
if self.mode == "PROBE":
if mower_ctr % 4 == 0 or mower_ctr == 0:
x_increase += 0
y_increase += 10
if mower_ctr % 2 == 0 and mower_ctr % 4 != 0:
x_increase -= 0
y_increase -= 7
if mower_ctr % 2 == 1:
x_increase += 3
y_increase += 0
mower_ctr += 1
des_x = self.curr_pose.pose.position.x + x_increase
des_y = self.curr_pose.pose.position.y + y_increase
if self.mode == "ROVER":
des_x = self.X_x[0] + (self.rover_velocity_x)
des_y = self.X_y[0] + (5 * self.rover_velocity_y)
#print(des_x,des_y)
#print("Detection Count:", self.detection_count)
z_increase = hover_dist - self.range # The integer value is chose such that the drone maintins the same value in units from the ground
print(z_increase)
des_z = self.curr_pose.pose.position.z + z_increase
pose_pub.publish(self.des_pose)
rate.sleep()
if self.range < 3.5 and self.mode == "DROP":
break
if self.detection_count >= 45 and self.mode == "ROVER":
print("InBreak")
break
if self.mode == "ROVER" and self.range < 10 and second_run == True:
print("Break second call")
break
if self.counter >= 25 and self.mode == "PROBE":
print("Breaking from the counter")
self.hover_x = self.curr_pose.pose.position.x
self.hover_y = self.curr_pose.pose.position.y
self.hover_z = self.curr_pose.pose.position.z
self.hover_loc = [
self.hover_x, self.hover_y, self.hover_z, 0, 0, 0, 0
]
rate.sleep()
break
def descent(self, hover_dist, delta_z, trunk_bool=False):
print("In Descent")
rate = rospy.Rate(10) # 10 Hz
pose_pub = rospy.Publisher('uav1/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
self.des_pose = self.copy_pose(self.curr_pose)
while self.range > hover_dist and not rospy.is_shutdown():
if self.mode == "PROBE":
err_x = self.destination_x - self.curr_pose.pose.position.x
err_y = self.destination_y - self.curr_pose.pose.position.y
x_change = (err_x * self.KP * 50)
y_change = -(err_y * self.KP * 100)
des = self.get_descent(x_change, y_change, delta_z)
self.vel_pub.publish(des)
if self.mode == "ROVER" and trunk_bool == False:
#print("DescentRover")
err_x = self.truck_target_x
err_y = self.truck_target_y
x_change = (err_x * self.KP * 50) + (
self.rover_velocity_x / 1.5) #8.5
#print("rover velocity", self.rover_velocity_x )
y_change = -(err_y * self.KP * 100) - (
2 * self.rover_velocity_y) #8.6
des = self.get_descent(x_change, y_change, -0.8)
self.vel_pub.publish(des)
if self.mode == "ROVER" and trunk_bool == True:
#print("DescentTrunk")
err_x = self.destination_x - self.curr_pose.pose.position.x
err_y = self.destination_y - self.curr_pose.pose.position.y
x_change = (err_x * self.KP * 50) + (
self.rover_velocity_x / 1.5) #8.5
#print("rover velocity", self.rover_velocity_x )
y_change = -(err_y * self.KP * 100) - (
1.8 * self.rover_velocity_y) #8.6
des = self.get_descent(x_change, y_change, -0.8)
self.vel_pub.publish(des)
#rate.sleep()
# des_x = self.destination_x + (self.rover_velocity_x*7.5) #10
# des_y = self.destination_y + (self.rover_velocity_y*7.5) #10
# des_z = self.rover_location_z + delta_z
# 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 = 0
# self.des_pose.pose.orientation.y = 0
# self.des_pose.pose.orientation.z = 0
# self.des_pose.pose.orientation.w = 0
# delta_z += delta_z
# pose_pub.publish(self.des_pose)
def controller(self):
while not rospy.is_shutdown():
if self.mode == "PROBE":
self.flytodestination(self.search_loc)
self.pattern(5, self.search_loc)
self.hover()
self.descent(0.7, -0.2)
self.attach()
self.mode = "DROP"
print(self.mode)
if self.mode == "DROP":
print("In Drop")
self.flytodestination(self.drop_loc)
self.pattern(3, self.drop_loc)
self.detach()
self.mode = "ROVER"
if self.mode == "ROVER":
#self.flytodestination([0, 0, 15, 0, 0, 0, 0])
location = [self.X_x[0], self.X_y[0], self.X_z[0]]
#print(location)
self.pattern(8, location, 1)
location = [self.X_x[0], self.X_y[0], self.X_z[0]]
self.pattern(8, location, 1, True)
self.descent(3, -0.75)
self.descent(0.5, -0.4, True)
self.mode = "END"
class GridToImageConverter():
def __init__(self):
self.image_topic = "wall_camera/image_raw"
self.image_info_topic = "wall_camera/camera_info"
self.point_topic = "/clicked_point"
self.frame_id = "/map"
self.occupancy
self.pixel_x = 350
self.pixel_y = 215
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
# Create the OpenCV display window for the RGB image
self.cv_window_name = self.image_topic
cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL)
cv.MoveWindow(self.cv_window_name, 25, 75)
# Create the cv_bridge object
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber(self.image_topic, Image, self.image_cb, queue_size = 1)
self.image_info_sub = rospy.Subscriber(self.image_info_topic, CameraInfo, self.image_info_cb, queue_size = 1)
self.point_sub = rospy.Subscriber(self.point_topic, PointStamped, self.point_cb, queue_size = 4)
self.points = []
self.ready_for_image = False
self.has_pic = False
self.camera_info_msg = None
self.tf_listener = tf.TransformListener()
# self.display_picture()
self.camera_model = PinholeCameraModel()
def point_cb(self, point_msg):
if len(self.points) < 4:
self.points.append(point_msg.point)
if len(self.points) == 1:
self.ready_for_image = True
print "Point stored from click: ", point_msg.point
def image_cb(self, img_msg):
if self.ready_for_image:
# Use cv_bridge() to convert the ROS image to OpenCV format
try:
frame = self.bridge.imgmsg_to_cv2(img_msg, "bgr8")
except CvBridgeError, e:
print e
# Convert the image to a Numpy array since most cv2 functions
# require Numpy arrays.
frame = np.array(frame, dtype=np.uint8)
print "Got array"
# Process the frame using the process_image() function
#display_image = self.process_image(frame)
# Display the image.
self.img = frame
# self.has_pic = True
if self.camera_info_msg is not None:
print "cx ", self.camera_model.cx()
print "cy ", self.camera_model.cy()
print "fx ", self.camera_model.fx()
print "fy ", self.camera_model.fy()
# Get the point from the clicked message and transform it into the camera frame
self.tf_listener.waitForTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0), rospy.Duration(0.5))
(trans,rot) = self.tf_listener.lookupTransform(img_msg.header.frame_id, self.frame_id, rospy.Time(0))
self.tfros = tf.TransformerROS()
tf_mat = self.tfros.fromTranslationRotation(trans, rot)
point_in_camera_frame = tuple(np.dot(tf_mat, np.array([self.points[0].x, self.points[0].y, self.points[0].z, 1.0])))[:3]
# Get the pixels related to the clicked point (the point must be in the camera frame)
pixel_coords = self.camera_model.project3dToPixel(point_in_camera_frame)
print "Pixel coords ", pixel_coords
# Get the unit vector related to the pixel coords
unit_vector = self.camera_model.projectPixelTo3dRay((int(pixel_coords[0]), int(pixel_coords[1])))
print "Unit vector ", unit_vector
# TF the unit vector of the pixel to the frame of the clicked point from the map frame
self.tf_listener.waitForTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0), rospy.Duration(0.5))
(trans,rot) = self.tf_listener.lookupTransform(self.frame_id, img_msg.header.frame_id, rospy.Time(0))
self.tfros = tf.TransformerROS()
tf_mat = self.tfros.fromTranslationRotation(trans, rot)
xyz = tuple(np.dot(tf_mat, np.array([unit_vector[0], unit_vector[1], unit_vector[2], 1.0])))[:3]
print "World xyz ", xyz
red = (0,125,255)
cv2.circle(self.img, (int(pixel_coords[0]), int(pixel_coords[1])), 30, red)
cv2.imshow(self.image_topic, self.img)
self.ready_for_image = False
self.points = []
# Process any keyboard commands
self.keystroke = cv.WaitKey(5)
if 32 <= self.keystroke and self.keystroke < 128:
cc = chr(self.keystroke).lower()
if cc == 'q':
# The user has press the q key, so exit
rospy.signal_shutdown("User hit q key to quit.")
class ColorDetectionService:
def __init__(self):
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.detected_objects = []
rospy.init_node("color_detection", log_level=rospy.DEBUG)
self.marker_type = int(rospy.get_param("marker_frame"))
print("Using marker frame id: {0}".format(self.marker_type))
self.frame = rospy.get_param("~image_frame")
self.image_topic = rospy.get_param("~image_topic")
rospy.Subscriber(self.image_topic, Image, self.callback)
self.pinhole_camera = PinholeCameraModel()
self.cam_info_topic = rospy.get_param("~cam_info_topic")
rospy.Subscriber(self.cam_info_topic, CameraInfo, self.update_cam_info)
# Marker service
rospy.wait_for_service('marker_pose', 5)
self.marker_pose_srv = rospy.ServiceProxy('marker_pose', ArMarkerPose)
rospy.Service("color_detection", ObjectPoses, self.get_color_positions)
def update_cam_info(self, message):
self.pinhole_camera.fromCameraInfo(message)
def color_filter(self, img, lower, upper):
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
lower_range = np.array(lower, dtype=np.uint8)
upper_range = np.array(upper, dtype=np.uint8)
mask = cv2.inRange(hsv, lower_range, upper_range)
return cv2.bitwise_and(hsv, hsv, mask=mask), mask
def find_contours(self, img, mask):
# find contours in the mask and initialize the current
# (x, y) center of the ball
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
contours = []
# only proceed if at least one contour was found
if len(cnts) == 0:
return contours
# find the largest contour in the mask, then use
# it to compute the minimum enclosing circle and
# centroid
for c in cnts:
((x, y), radius) = cv2.minEnclosingCircle(c)
# only proceed if the radius meets a minimum size
if radius < 4 or radius > 9:
continue
#rospy.logdebug("Min enclosing circle: {0}, {1}, {2}".format(x, y, radius))
M = cv2.moments(c)
if M["m00"] != 0:
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
#rospy.logdebug("Moment: {0}, {1}".format(center[0], center[1]))
# draw the circle and centroid on the frame,
# then update the list of tracked points
contours.append((center[0], center[1], int(radius)))
return contours
def detect_color(self, img, lower, upper):
copy_img = copy.copy(img)
new_img, new_mask = self.color_filter(copy_img, lower, upper)
return self.find_contours(new_img, new_mask)
def detect_colors(self, img):
#cv2.medianBlur(img, 5)
detected_objects = {}
yellowObjects = self.detect_color(img, [20, 60, 120], [50, 100, 140])
detected_objects["yellow"] = yellowObjects
redObjects = self.detect_color(img, [0, 100, 70], [10, 150, 150])
detected_objects["red"] = redObjects
blueObjects = self.detect_color(img, [100, 140, 60], [120, 160, 80])
detected_objects["blue"] = blueObjects
greenObjects = self.detect_color(img, [80, 100, 40], [100, 110, 60])
detected_objects["green"] = greenObjects
for color in detected_objects:
for obj in detected_objects[color]:
obj = np.uint16(np.around(obj))
if color == "yellow":
bgr = (0, 255, 255)
elif color == "red":
bgr = (0, 0, 255)
elif color == "blue":
bgr = (255, 0, 0)
elif color == "green":
bgr = (0, 255, 0)
cv2.circle(img, (obj[0], obj[1]), obj[2], bgr, 2)
cv2.circle(img, (obj[0], obj[1]), 2, bgr, 3)
global img_display
img_display = img
cv2.imshow('colors', img)
key = cv2.waitKey(1) & 0xFF
#rospy.logdebug("Detected objects: {0}".format(detected_objects))
return detected_objects
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgra8")
except CvBridgeError as e:
rospy.logerr(e)
self.detected_objects = self.detect_colors(cv_image)
#print(self.detected_objects)
def get_color_positions(self, request):
response = ObjectPosesResponse()
if self.detected_objects is None:
return response
if len(self.detected_objects) == 0:
rospy.logdebug("No object detected")
return response
rospy.wait_for_service('marker_pose', 5.0)
marker_pose_request = ArMarkerPoseRequest(frame=request.frame,
marker_type=self.marker_type)
marker_pose_response = self.marker_pose_srv(marker_pose_request)
marker_pose = marker_pose_response.pose
if marker_pose is None or marker_pose.pose.position.x == 0:
rospy.logdebug("Couldn't find marker frame")
return response
colors = self.detected_objects.keys()
objects = self.detected_objects.values()
for c, objs in zip(colors, objects):
for o in objs:
ray = self.pinhole_camera.projectPixelTo3dRay((o[0], o[1]))
point = self.ray_to_3dpoint(ray, marker_pose, request.frame)
pose = PoseStamped(header=Header(frame_id=self.frame,
stamp=rospy.Time(0)),
pose=Pose(position=point))
self.listener.waitForTransform(request.frame, self.frame,
rospy.Time(0),
rospy.Duration(3.0))
transformed_pose = self.listener.transformPose(
request.frame, pose)
#rospy.logdebug("Point in base frame: {0}".format(transformed_pose.pose.position))
response.colors.append(c)
response.poses.append(transformed_pose)
response
return response
def ray_to_3dpoint(self, ray, marker_pose, base_frame):
point = Point()
#rospy.logdebug("Ray: {0}".format(ray))
self.listener.waitForTransform(base_frame, self.frame, rospy.Time(0),
rospy.Duration(3.0))
p, q = self.listener.lookupTransform(base_frame, self.frame,
rospy.Time(0))
#rospy.logdebug("Translation: {0}".format(p))
R = tf.transformations.quaternion_matrix(q)
#rospy.logdebug("Rotation: {0}".format(R))
#rospy.logdebug("AR tag: {0}".format(marker_pose.pose.position))
rray = Vector3Stamped(header=Header(frame_id=self.frame,
stamp=rospy.Time(0)),
vector=Vector3(x=ray[0], y=ray[1], z=ray[2]))
tray = self.listener.transformVector3(base_frame, rray)
#rospy.logdebug("True transformed ray: {0}".format(tray.vector))
#rospy.logdebug("Transformed ray: {0}".format(np.dot(R[:3, :3], np.array(ray))))
t = (marker_pose.pose.position.z - p[2]) / np.dot(
R[:3, :3], np.array(ray))[2]
#rospy.logdebug("Parameter t: {0}".format(t))
point.x = ray[0] * t
point.y = ray[1] * t
point.z = ray[2] * t
#rospy.logdebug("Point in camera frame: {0}".format(point))
return point
def run(self):
rospy.spin()
def DistToColoredFunc(color, needToTurn):
global ObjFound
camInfo = rospy.wait_for_message('/usb_cam/camera_info', CameraInfo)
IMAGE = rospy.wait_for_message('/usb_cam/image_raw', Image)
camera = PinholeCameraModel()
camera.fromCameraInfo(camInfo)
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(IMAGE, 'bgr8')
gau_blur = cv2.GaussianBlur(cv_image, (3, 3), 0)
######################################################################
if (color == 'blue'):
lower = np.array([135, 115, 80], dtype="uint8")
upper = np.array([255, 160, 120], dtype="uint8")
elif (color == 'red'):
lower = np.array([0, 25, 110], dtype="uint8")
upper = np.array([50, 100, 255], dtype="uint8")
elif (color == 'green'):
lower = np.array([75, 120, 80], dtype="uint8")
upper = np.array([110, 255, 110], dtype="uint8")
else:
exit()
mask_image = cv2.inRange(gau_blur, lower, upper)
kernelOpen = np.ones((5, 5))
kernelClose = np.ones((20, 20))
maskOpen = cv2.morphologyEx(mask_image, cv2.MORPH_OPEN, kernelOpen)
final_mask = cv2.morphologyEx(maskOpen, cv2.MORPH_CLOSE, kernelClose)
##############################################################
img2, contours, h = cv2.findContours(final_mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
contour_area = False
for i in range(len(contours)):
if (cv2.contourArea(contours[i]) > 40000):
contour_area = True
if (len(contours) != 0 and contour_area):
ObjFound = True
cnt = contours[0]
for i in range(len(contours)):
if cv2.contourArea(contours[i]) > cv2.contourArea(
cnt): # we take the biggest one
cnt = contours[i]
M = cv2.moments(cnt)
x_center_obj = int(M['m10'] / M['m00'])
y_center_obj = 240
center_obj = (x_center_obj, y_center_obj)
image_center = (320, 240)
ray_obj = camera.projectPixelTo3dRay(center_obj)
ray_img = camera.projectPixelTo3dRay(image_center)
# angle between two vectors - by formula. We'll get the cos of the angle so we'll do arccos
radians = math.acos(
np.dot(ray_obj, ray_img) /
(np.linalg.norm(ray_obj) * np.linalg.norm(ray_img)))
degrees = int(math.degrees(radians))
#if not needToTurn:
if abs(x_center_obj - image_center[0]) > 100:
degrees = degrees + 5
degrees = degrees % 360
elif abs(x_center_obj - image_center[0]) > 40:
degrees = degrees + 2
degrees = degrees % 360
if x_center_obj >= image_center[0]:
degrees = 360 - degrees
if degrees == 360:
degrees = 0
if needToTurn:
laserInfo = rospy.wait_for_message('/scan', LaserScan)
rotate(-degrees) #turn the robot
degrees = 0
if laserInfo.ranges[0] > 0.55:
laserInfo = rospy.wait_for_message('/scan', LaserScan)
#rospy.sleep(10)
distance = distanceCalculation(degrees)
if distance == 0:
return 0
return distance
else:
ObjFound = False
return None
class image_conv:
def __init__(self):
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber(
"/thorvald_001/kinect2_camera/hd/image_color_rect", Image,
self.callback)
self.image_info = rospy.Subscriber(
"/thorvald_001/kinect2_camera/hd/camera_info", CameraInfo,
self.cam_info_callback)
self.cam = None
def cam_info_callback(self, data):
if self.cam is None:
print('initialise caminfo')
self.cam = PinholeCameraModel()
self.cam.fromCameraInfo(data)
def callback(self, data):
image = self.bridge.imgmsg_to_cv2(data, "bgr8")
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
blur = cv2.GaussianBlur(hsv, ksize=(17, 17), sigmaX=10)
up_w = np.array([40, 0, 0])
lw_w = np.array([180, 120, 255])
mask = cv2.inRange(blur, up_w, lw_w)
res = cv2.bitwise_and(image, image, mask=mask)
gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
ret, thresh = cv2.threshold(gray, 10, 255, 0)
cv2.imshow('thresh', thresh)
result = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
print("Number of contours=" + str(len(result)))
if len(result) == 2:
contours, hierarchy = result
elif len(result) == 3:
im2, contours, hierarchy = result
threshold_area = 500
filtered_contours = []
for count in contours:
area = cv2.contourArea(count)
print(area)
if area > threshold_area:
filtered_contours.append(count)
cv2.drawContours(image, filtered_contours, -1, (0, 255, 0), 3)
rects = []
for count in filtered_contours:
x, y, w, h = cv2.boundingRect(count)
rects.append([x, y, w, h])
rects.append([x, y, w, h])
cv2.rectangle(image, (x, y), (x + w, y + h), (0, 255, 0), 8)
contours_points = []
contours_boxes, weights = cv2.groupRectangles(rects, 1, 0.2)
for rect in contours_boxes:
middle = (x + w / 2, y + h / 2)
if self.cam:
xa, ya, za = self.cam.projectPixelTo3dRay(middle)
print(xa, ya, za)
pts = xa / 2, ya / 2, za / 2
print(pts)
contours_points.append(middle)
cv2.circle(image, middle, 7, (255, 255, 255), -1)
x, y, w, h = rect
cv2.rectangle(image, (x, y), (x + w, y + h), (255, 0, 0), 4)
cv2.imshow('image', image)
cv2.imshow('mask', mask)
cv2.waitKey(1)
def get_direction(self, camera_info, px, py):
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
cx, cy, cz = camera_model.projectPixelTo3dRay((px, py))
return cx, cy, cz
