def localize_detection(self, detection):
"""
2d image detection -> 3d point in [map frame]
"""
u = detection.x + detection.width / 2
v = detection.y + detection.height / 2
camera_info = None
best_time = 100
for ci in self.camera_infos:
time = abs(ci.header.stamp.to_sec() -
detection.header.stamp.to_sec())
if time < best_time:
camera_info = ci
best_time = time
if not camera_info or best_time > 1:
return False, None
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
point = Point(
((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(),
((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(),
1)
localization = self.localize(detection.header, point,
self.region_scope)
if not localization:
return False, None
point_trans = None
print "Face localized ", localization.pose.position.x, localization.pose.position.y
if localization.pose.position.x == 0.0:
print "Ignoring this one!"
return False, None
try:
#(pos_trans, rot) = self.tf_listener.lookupTransform('/map', detection.header.frame_id, rospy.Time(0))
point_trans = self.tf_listener.transformPoint(
'/map',
PointStamped(detection.header, localization.pose.position))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
if point_trans == None: # transformation failed
return False, None
print "Face detected at %d,%d" % (point_trans.point.x,
point_trans.point.y)
return True, point_trans.point
def detections_callback(self, detection):
global counter
global maxDistance #the maximal distance between two points required for them to be considered the same detection
global alreadyDetected
global n
global lastAdded
global detected
#global tf
global numFaces
u = detection.x + detection.width / 2
v = detection.y + detection.height / 2
camera_info = None
best_time = 100
for ci in self.camera_infos:
time = abs(ci.header.stamp.to_sec() -
detection.header.stamp.to_sec())
if time < best_time:
camera_info = ci
best_time = time
if not camera_info or best_time > 1:
print("Best time is higher than 1")
return
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
point = Point(
((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(),
((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(),
1)
localization = self.localize(detection.header, point,
self.region_scope)
transformedPoint = point
if not localization:
return
now = detection.header.stamp
self.tf.waitForTransform("camera_rgb_optical_frame", "map", now,
rospy.Duration(5.0))
m = geometry_msgs.msg.PoseStamped()
m.header.frame_id = "camera_rgb_optical_frame"
m.pose = localization.pose
m.header.stamp = detection.header.stamp
transformedPoint = self.tf.transformPose("map", m)
if (localization.pose.position.x != 0.0):
#print("Localisation: ", localization)
print("Transformation: ", transformedPoint)
#print("Point: ", point)
if counter == 0:
#print("Counter is zero.")
counter += 1
lastAdded = transformedPoint
#print("Adding the point to the array.")
#alreadyDetected.append(transformedPoint)
#print(len(alreadyDetected))
else:
#print("Counter: ", counter)
dist = self.distance(transformedPoint, lastAdded)
#print("Number of detected faces so far: ", len(detected))
print("Distance is ", dist)
if dist <= maxDistance:
#print("-----------Less then max----------------------------------")
if counter < 2 or counter >= 2:
#alreadyDetected.append(transformedPoint)
#print(len(alreadyDetected))
lastAdded = transformedPoint
counter += 1
beenDetected = False
for p in detected:
#print("Checking already detected")
#print("Distance: ", self.distance(p, transformedPoint))
if self.distance(p,
transformedPoint) <= maxDistance:
#print("This face has already been detected")
#print("Distance: ", self.distance(p, transformedPoint))
beenDetected = True
break
if (beenDetected == False):
print(
"-----------------Good to go------------------------"
)
detected.append(lastAdded)
marker = Marker()
#print("Localisation: ", localization)
marker.header.stamp = detection.header.stamp
marker.header.frame_id = detection.header.frame_id
marker.pose = localization.pose
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.frame_locked = False
marker.lifetime = rospy.Duration.from_sec(1)
marker.id = self.marker_id_counter
marker.scale = Vector3(0.1, 0.1, 0.1)
marker.color = ColorRGBA(1, 0, 0, 1)
self.markers.markers.append(marker)
self.marker_id_counter += 1
self.soundhandle.say("I detected a face.",
blocking=False)
print("Number of detected faces: ", len(detected))
if len(detected) == numFaces:
#publish stop
#rospy.init_node('mapper', anonymous=True)
#msg = String()
#msg.data = "Found all faces."
print("Sending shutdown")
self.pub.publish("Found all faces.")
else:
#print("-----------------------------------------More then max----")
lastAdded = transformedPoint
else:
print("Localisation: ", localization)
def detections_callback(self, detection):
global counter
global maxDistance #the maximal distance between two points required for them to be considered the same detection
global alreadyDetected
global n
global lastAdded
global detected
global numFaces
global brojach
global avgx
global avgy
global avgr
global flag
global maxHeight
global minHeight
global maxDistRing
if flag == 0:
if brojach < 10:
avgx = avgx + detection.pose.position.x
avgy = avgy + detection.pose.position.y
avgr = avgr + detection.pose.position.z
brojach = brojach + 1
return
if brojach == 10:
tp = detection
avgx = avgx / 10
avgy = avgy / 10
avgr = avgr / 10
camera_info = None
best_time = 100
for ci in self.camera_infos:
time = abs(ci.header.stamp.to_sec() -
detection.header.stamp.to_sec())
if time < best_time:
camera_info = ci
best_time = time
if not camera_info or best_time > 1:
print("Best time is higher than 1")
tp.pose.position.z = -1
self.pub_avg_ring.publish(tp)
flag = 1
return
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
p = Point(
((avgx - camera_model.cx()) - camera_model.Tx()) /
camera_model.fx(),
((avgy + avgr - camera_model.cy()) - camera_model.Ty()) /
camera_model.fy(), 0)
#p2 = Point(((avgx - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(),((avgy -avgr - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(), 0)
avgx = avgy = avgr = 0
lp = self.localize(detection.header, p1, self.region_scope)
#lp2 = self.localize(detection.header, p2, self.region_scope)
self.tf.waitForTransform(detection.header.frame_id, "map",
rospy.Time.now(), rospy.Duration(5.0))
mp = geometry_msgs.msg.PoseStamped()
mp.header.frame_id = detection.header.frame_id
mp.pose = lp.pose
mp.header.stamp = detection.header.stamp
tp = self.tf.transformPose("map", mp1)
#mp2 = geometry_msgs.msg.PoseStamped()
#mp2.header.frame_id = detection.header.frame_id
#mp2.pose = lp2.pose
#mp2.header.stamp = detection.header.stamp
#tp2 = self.tf.transformPose("map", mp2)
now2 = rospy.Time.now()
self.tf2.waitForTransform("base_link", "map", now2,
rospy.Duration(5.0))
robot = geometry_msgs.msg.PoseStamped()
robot.header.frame_id = "base_link"
robot.pose.position.x = 0
robot.pose.position.y = 0
robot.header.stamp = now2
robotmap = self.tf2.transformPose("map", robot)
if lp1.pose.position.z != 0:
dp1 = self.distance(robotmap, tp)
if dp > maxDistRing:
print("Distance too big: ", dp)
tp.pose.position.z = -1
if tp.pose.position.z > maxHeight + 0.2 or tp.pose.position.z < minHeight:
print("Height is wrong: ", tp.pose.position.z)
tp.pose.position.z = -1
#if ((point1.y - point2.y)/2 > 0.7 or (point1.y - point2.y)/2 < 0.5):
# tp.pose.position.z = -1
#if (point1.z > 0.5 or point1.z < 0.3):
# tp.pose.position.z = -1
#if (point2.z > 0.5 or point2.z < 0.3): # visina
# tp.pose.position.z = -1
else:
print("Localizer failed")
tp.pose.position.z = -1
self.pub_avg_ring.publish(tp)
flag = 1
return
if flag == 1:
u = detection.pose.position.x
#u = avgx+avgr
v = detection.pose.position.y + detection.pose.position.z
#v = avgy
w = detection.pose.position.x
#w = avgx-avgr
q = detection.pose.position.y - detection.pose.position.z
g = detection.pose.position.x
#w = avgx-avgr
h = detection.pose.position.y + detection.pose.position.z + 3
r = detection.pose.position.x
#w = avgx-avgr
t = detection.pose.position.y + detection.pose.position.z - 3
#q = avgy
camera_info = None
best_time = 100
for ci in self.camera_infos:
time = abs(ci.header.stamp.to_sec() -
detection.header.stamp.to_sec())
if time < best_time:
camera_info = ci
best_time = time
if not camera_info or best_time > 1:
print("Best time is higher than 1")
return
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
point1 = Point(((u - camera_model.cx()) - camera_model.Tx()) /
camera_model.fx(),
((v - camera_model.cy()) - camera_model.Ty()) /
camera_model.fy(), 1)
point2 = Point(((w - camera_model.cx()) - camera_model.Tx()) /
camera_model.fx(),
((q - camera_model.cy()) - camera_model.Ty()) /
camera_model.fy(), 1)
point3 = Point(((g - camera_model.cx()) - camera_model.Tx()) /
camera_model.fx(),
((h - camera_model.cy()) - camera_model.Ty()) /
camera_model.fy(), 1)
point4 = Point(((r - camera_model.cx()) - camera_model.Tx()) /
camera_model.fx(),
((t - camera_model.cy()) - camera_model.Ty()) /
camera_model.fy(), 1)
localization1 = self.localize(detection.header, point1,
self.region_scope)
localization2 = self.localize(detection.header, point2,
self.region_scope)
localization3 = self.localize(detection.header, point3,
self.region_scope)
localization4 = self.localize(detection.header, point4,
self.region_scope)
if not (localization1 or localization2 or localization3
or localization):
print("Localization failed")
return
now = detection.header.stamp
self.tf.waitForTransform(detection.header.frame_id, "map", now,
rospy.Duration(5.0))
m = geometry_msgs.msg.PoseStamped()
m.header.frame_id = detection.header.frame_id
m.pose = localization1.pose
m.header.stamp = detection.header.stamp
transformedPoint1 = self.tf.transformPose("map", m)
m2 = geometry_msgs.msg.PoseStamped()
m2.header.frame_id = detection.header.frame_id
m2.pose = localization2.pose
m2.header.stamp = detection.header.stamp
transformedPoint2 = self.tf.transformPose("map", m2)
m3 = geometry_msgs.msg.PoseStamped()
m3.header.frame_id = detection.header.frame_id
m3.pose = localization3.pose
m3.header.stamp = detection.header.stamp
transformedPoint3 = self.tf.transformPose("map", m3)
m4 = geometry_msgs.msg.PoseStamped()
m4.header.frame_id = detection.header.frame_id
m4.pose = localization4.pose
m4.header.stamp = detection.header.stamp
transformedPoint4 = self.tf.transformPose("map", m4)
now2 = rospy.Time.now()
self.tf2.waitForTransform("base_link", "map", now2,
rospy.Duration(5.0))
robot = geometry_msgs.msg.PoseStamped()
robot.header.frame_id = "base_link"
robot.pose.position.x = 0
robot.pose.position.y = 0
robot.header.stamp = now2
robotmap = self.tf2.transformPose("map", robot)
if localization1.pose.position.z != 0:
dist1 = self.distance(robotmap, transformedPoint1)
else:
dist1 = 100000
if localization2.pose.position.z != 0:
dist2 = self.distance(robotmap, transformedPoint2)
else:
dist2 = 100000
if localization3.pose.position.z != 0:
dist3 = self.distance(robotmap, transformedPoint3)
else:
dist3 = 100000
if localization4.pose.position.z != 0:
dist4 = self.distance(robotmap, transformedPoint4)
else:
dist4 = 100000
# find smallest distance to a point.
dist = dist1
transformedPoint = transformedPoint1
if dist2 < dist:
dist = dist2
transformedPoint = transformedPoint2
if dist3 < dist:
dist = dist3
transformedPoint = transformedPoint3
if dist4 < dist:
dist = dist4
transformedPoint = transformedPoint4
print("distance: ", dist)
if (dist < maxDistRing):
radius = abs((transformedPoint1.pose.position.y -
transformedPoint2.pose.position.y) / 2)
print("radius: ", radius)
print("height: ", transformedPoint.pose.position.z)
if (dist1 < maxDistRing
and dist2 < maxDistRing) or (dist1 > maxDistRing
and dist2 > maxDistRing):
print("Checking radius")
if (radius > 0.7 or radius < 0.3):
print("Wrong radius")
return
else:
print("Cant use radius")
if (transformedPoint.pose.position.z > maxHeight
or transformedPoint.pose.position.z < minHeight):
print("Wrong height")
return
print("start checking")
print("Detected rings: ", len(detected))
if len(detected) == 0:
beenDetected = False
else:
beenDetected = False
for p in detected:
if self.distance(p, transformedPoint) <= maxDistance:
print("Already detected ring!")
beenDetected = True
break
if (beenDetected == False):
detected.append(transformedPoint)
print("Publishing new ring")
self.pub_ring.publish(transformedPoint)
marker = Marker()
marker.header.stamp = detection.header.stamp
marker.header.frame_id = transformedPoint.header.frame_id
marker.pose.position.x = transformedPoint.pose.position.x
marker.pose.position.y = transformedPoint.pose.position.y
marker.pose.position.z = transformedPoint.pose.position.z
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.frame_locked = False
marker.lifetime = rospy.Duration.from_sec(1)
marker.id = self.marker_id_counter
marker.scale = Vector3(0.1, 0.1, 0.1)
marker.color = ColorRGBA(1, 0, 0, 1)
self.markers.markers.append(marker)
self.marker_id_counter += 1
print("Number of detected rings: ", len(detected))
#if len(detected) == numFaces:
# print("Sending shutdown")
else:
print("Too far away")
def detections_callback(self, detection):
global counter
global maxDistance #the maximal distance between two points required for them to be considered the same detection
global alreadyDetected
global n
global lastAdded
global detected
global numFaces
global brojach
global avgx
global avgy
global avgr
global flag
if flag == 0:
if brojach < 10:
avgx = avgx + detection.pose.position.x
avgy = avgy + detection.pose.position.y
avgr = avgr + detection.pose.position.z
brojach = brojach + 1
return
if brojach == 10:
avgx = avgx / 10
avgy = avgy / 10
avgr = avgr / 10
p = Point(((avgx - camera_model.cx()) - camera_model.Tx()) /
camera_model.fx(),
((avgy - camera_model.cy()) - camera_model.Ty()) /
camera_model.fy(), avgr)
lp = self.localize(detection.header, p, self.region_scope)
self.tf.waitForTransform(detection.header.frame_id, "map",
rospy.Time.now(), rospy.Duration(5.0))
mp = geometry_msgs.msg.PoseStamped()
mp.header.frame_id = detection.header.frame_id
mp.pose = lp.pose
mp.header.stamp = detection.header.stamp
tp = self.tf.transformPose("map", mp)
if lp.pose.position.x != 0:
dp = self.distance(robotmap, tp)
if dp > 1.5:
tp.pose.position.z = -1
#if ((point1.y - point2.y)/2 > 0.7 or (point1.y - point2.y)/2 < 0.5):
# tp.pose.position.z = -1
#if (point1.z > 0.5 or point1.z < 0.3):
# tp.pose.position.z = -1
#if (point2.z > 0.5 or point2.z < 0.3): # visina
# tp.pose.position.z = -1
else:
tp.pose.position.z = -1
self.pub_avg_ring.publish(tp)
flag = 1
return
if flag == 1:
u = detection.pose.position.x
#u = avgx+avgr
v = detection.pose.position.y + detection.pose.position.z
#v = avgy
w = detection.pose.position.x
#w = avgx-avgr
q = detection.pose.position.y - detection.pose.position.z
g = detection.pose.position.x
#w = avgx-avgr
h = detection.pose.position.y + detection.pose.position.z + 4
r = detection.pose.position.x
#w = avgx-avgr
t = detection.pose.position.y + detection.pose.position.z - 4
#q = avgy
camera_info = None
best_time = 100
for ci in self.camera_infos:
time = abs(ci.header.stamp.to_sec() -
detection.header.stamp.to_sec())
if time < best_time:
camera_info = ci
best_time = time
if not camera_info or best_time > 1:
print("Best time is higher than 1")
return
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
point1 = Point(((u - camera_model.cx()) - camera_model.Tx()) /
camera_model.fx(),
((v - camera_model.cy()) - camera_model.Ty()) /
camera_model.fy(), 1)
point2 = Point(((w - camera_model.cx()) - camera_model.Tx()) /
camera_model.fx(),
((q - camera_model.cy()) - camera_model.Ty()) /
camera_model.fy(), 1)
point3 = Point(((g - camera_model.cx()) - camera_model.Tx()) /
camera_model.fx(),
((h - camera_model.cy()) - camera_model.Ty()) /
camera_model.fy(), 1)
point4 = Point(((r - camera_model.cx()) - camera_model.Tx()) /
camera_model.fx(),
((t - camera_model.cy()) - camera_model.Ty()) /
camera_model.fy(), 1)
localization1 = self.localize(detection.header, point1,
self.region_scope)
localization2 = self.localize(detection.header, point2,
self.region_scope)
localization3 = self.localize(detection.header, point3,
self.region_scope)
localization4 = self.localize(detection.header, point4,
self.region_scope)
if not localization1:
return
# calculate center
center = Point(localization1.pose.position.x,
localization1.pose.position.y,
localization1.pose.position.z)
now = detection.header.stamp
self.tf.waitForTransform(detection.header.frame_id, "map", now,
rospy.Duration(5.0))
m = geometry_msgs.msg.PoseStamped()
m.header.frame_id = detection.header.frame_id
#m.pose.position.x = center.x
#m.pose.position.y = center.y
m.pose = localization1.pose
m.header.stamp = detection.header.stamp
transformedPoint1 = self.tf.transformPose("map", m)
m2 = geometry_msgs.msg.PoseStamped()
m2.header.frame_id = detection.header.frame_id
m2.pose = localization2.pose
m2.header.stamp = detection.header.stamp
transformedPoint2 = self.tf.transformPose("map", m2)
m3 = geometry_msgs.msg.PoseStamped()
m3.header.frame_id = detection.header.frame_id
m3.pose = localization3.pose
m3.header.stamp = detection.header.stamp
transformedPoint3 = self.tf.transformPose("map", m3)
m4 = geometry_msgs.msg.PoseStamped()
m4.header.frame_id = detection.header.frame_id
m4.pose = localization4.pose
m4.header.stamp = detection.header.stamp
transformedPoint4 = self.tf.transformPose("map", m4)
now2 = rospy.Time.now()
self.tf2.waitForTransform("base_link", "map", now2,
rospy.Duration(5.0))
robot = geometry_msgs.msg.PoseStamped()
robot.header.frame_id = "base_link"
robot.pose.position.x = 0
robot.pose.position.y = 0
#robot.pose = localization1.pose
robot.header.stamp = now2
robotmap = self.tf2.transformPose("map", robot)
#dist = self.distance(robotmap,transformedPoint)
transformedPoint = transformedPoint1
if localization1.pose.position.x != 0:
dist1 = self.distance(robotmap, transformedPoint1)
else:
dist1 = 100000
if localization2.pose.position.x != 0:
dist2 = self.distance(robotmap, transformedPoint2)
transformedPoint = transformedPoint2
else:
dist2 = 100000
if localization3.pose.position.x != 0:
dist3 = self.distance(robotmap, transformedPoint3)
transformedPoint = transformedPoint3
else:
dist3 = 100000
if localization4.pose.position.x != 0:
dist4 = self.distance(robotmap, transformedPoint4)
transformedPoint = transformedPoint4
else:
dist4 = 100000
# find smallest distance to a point.
dist = dist1
if dist2 < dist:
dist = dist2
if dist3 < dist:
dist = dist3
if dist4 < dist:
dist = dist4
print("distance: ", dist)
if (dist < 1.5):
rad = abs((point1.y - point2.y) / 2)
print("radius: ", rad)
print("height p1: ", point1.z)
print("height p2: ", point2.z)
#if ((point1.y - point2.y)/2 > 0.7 or (point1.y - point2.y)/2 < 0.5):
# return
#if (point1.z > 0.5 or point1.z < 0.3):
# return
#if (point2.z > 0.5 or point2.z < 0.3): # visina
# return
self.pub.publish(transformedPoint)
print("start checking")
print(len(detected))
if len(detected) == 0:
detected.append(transformedPoint)
beenDetected = False
else:
beenDetected = False
for p in detected:
if self.distance(p, transformedPoint) <= maxDistance:
print("Already detected ring!")
beenDetected = True
break
if (beenDetected == False):
print("Publishing new ring")
self.pub_ring.publish(transformedPoint)
marker = Marker()
marker.header.stamp = detection.header.stamp
marker.header.frame_id = transformedPoint.header.frame_id
marker.pose.position.x = transformedPoint.pose.position.x
marker.pose.position.y = transformedPoint.pose.position.y
marker.pose.position.z = transformedPoint.pose.position.z
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.frame_locked = False
marker.lifetime = rospy.Duration.from_sec(1)
marker.id = self.marker_id_counter
marker.scale = Vector3(0.1, 0.1, 0.1)
marker.color = ColorRGBA(1, 0, 0, 1)
self.markers.markers.append(marker)
self.marker_id_counter += 1
print("Number of detected rings: ", len(detected))
if len(detected) == numFaces:
print("Sending shutdown")
else:
print("Just zeros")
def detections_callback(self, detection):
global counter
global maxDistance #the maximal distance between two points required for them to be considered the same detection
global alreadyDetected
global n
global lastAdded
global detected
global numFaces
u = detection.x + detection.width / 2
v = detection.y + detection.height / 2
camera_info = None
best_time = 100
for ci in self.camera_infos:
time = abs(ci.header.stamp.to_sec() -
detection.header.stamp.to_sec())
if time < best_time:
camera_info = ci
best_time = time
if not camera_info or best_time > 1:
print("Best time is higher than 1")
return
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
point = Point(
((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(),
((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(),
1)
localization = self.localize(detection.header, point,
self.region_scope)
transformedPoint = point
if not localization:
return
now = detection.header.stamp
self.tf.waitForTransform("camera_rgb_optical_frame", "map", now,
rospy.Duration(5.0))
m = geometry_msgs.msg.PoseStamped()
m.header.frame_id = "camera_rgb_optical_frame"
m.pose = localization.pose
m.header.stamp = detection.header.stamp
transformedPoint = self.tf.transformPose("map", m)
if (localization.pose.position.x != 0.0):
#print("Transformation: ", transformedPoint)
beenDetected = False
if counter == 0:
lastAdded = transformedPoint
else:
lastAdded = transformedPoint
for p in detected:
if self.distance(p, transformedPoint) <= maxDistance:
beenDetected = True
break
if (beenDetected == False):
counter += 1
print("-----------------Good to go------------------------")
detected.append(lastAdded)
self.pub_face.publish(transformedPoint)
marker = Marker()
marker.header.stamp = detection.header.stamp
marker.header.frame_id = detection.header.frame_id
marker.pose = localization.pose
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.frame_locked = False
marker.lifetime = rospy.Duration.from_sec(1)
marker.id = self.marker_id_counter
marker.scale = Vector3(0.1, 0.1, 0.1)
marker.color = ColorRGBA(1, 0, 0, 1)
self.markers.markers.append(marker)
self.marker_id_counter += 1
#self.soundhandle.say("I detected a face.", blocking = False)
print("Number of detected faces: ", len(detected))
lastAdded = transformedPoint
