def pub_roi(self, l_roi):
"""
Publish the roi of the maximum sized area
:args l_roi: a list of RegionOfInterst Objects
"""
logging.debug("attempting to publish to the roi topic")
#print("attempting to publish to the roi topic")
# Short circuit of no rois
if len(l_roi) == 0:
logging.debug("no roi to publish")
print("no roi to publish")
return
try:
largest = max(l_roi, key=lambda p: p[2] * p[3])
# Short Circuit if invalid dimensions
if largest[0] < 0 or largest[1] < 0:
return
roi = RegionOfInterest()
roi.x_offset = largest[0]
roi.y_offset = largest[1]
roi.width = largest[2]
roi.height = largest[3]
except Exception as e:
logging.warning(e)
print()
print(e)
print(largest)
print()
roi = RegionOfInterest()
try:
self.image_pub_roi.publish(roi)
logging.debug("ROI published")
print("ROI published")
roi = RegionOfInterest()
except Exception as e:
logging.warning(e)
print()
print(e)
print(largest)
print()
roi = RegionOfInterest()
return
class cvBridgeDemo():
def __init__(self):
self.node_name = "crop_row_publisher"
rospy.init_node(self.node_name)
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
# Create the OpenCV display window for the RGB image
self.cv_window_name = self.node_name
# Initialize the Region of Interest and its publisher
self.ROI = RegionOfInterest()
self.roi_pub = rospy.Publisher("/roi", RegionOfInterest, queue_size=1)
# Create the cv_bridge object
self.bridge = CvBridge()
# Subscribe to the camera image and depth topics and set
# the appropriate callbacks
self.image_sub = rospy.Subscriber("input_rgb_image",
Image,
self.image_callback,
queue_size=1)
rospy.loginfo("Waiting for image topics...")
rospy.wait_for_message("input_rgb_image", Image)
rospy.loginfo("Ready.")
def image_callback(self, ros_image):
# Use cv_bridge() to convert the ROS image to OpenCV format
try:
frame = self.bridge.imgmsg_to_cv2(ros_image, "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)
# Process the frame using the process_image() function
(crop_lines, x_offset, width) = self.process_image(frame)
ROI = RegionOfInterest()
ROI.x_offset = x_offset
ROI.width = width
self.roi_pub.publish(ROI)
# Display the image and the detected lines.
cv2.imshow(self.node_name, cv2.addWeighted(frame, 1, crop_lines, 1,
0.0))
# Process any keyboard commands
self.keystroke = cv2.waitKey(5)
if self.keystroke != -1:
cc = chr(self.keystroke & 255).lower()
if cc == 'q':
# The user has press the q key, so exit
rospy.signal_shutdown("User hit q key to quit.")
def rosmsg(self):
""":obj:`sensor_msgs.CamerInfo` : Returns ROS CamerInfo msg
"""
msg_header = Header()
msg_header.frame_id = self._frame
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = self._height
msg.width = self._width
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [
self._fx, 0.0, self._cx, 0.0, self._fy, self._cy, 0.0, 0.0, 1.0
]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [
self._fx, 0.0, self._cx, 0.0, 0.0, self._fx, self._cy, 0.0, 0.0,
0.0, 1.0, 0.0
]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def to_roi(top_left, bottom_right):
msg = RegionOfInterest()
msg.x_offset = round(top_left[0])
msg.y_offset = round(top_left[1])
msg.width = round(abs(bottom_right[0] - top_left[0]))
msg.height = round(abs(bottom_right[1] - top_left[1]))
return msg
def camera_info(self):
msg_header = Header()
msg_header.frame_id = "f450/robot_camera_down"
msg_roi = RegionOfInterest()
msg_roi.x_offset = 0
msg_roi.y_offset = 0
msg_roi.height = 0
msg_roi.width = 0
msg_roi.do_rectify = 0
msg = CameraInfo()
msg.header = msg_header
msg.height = 480
msg.width = 640
msg.distortion_model = 'plumb_bob'
msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.K = [1.0, 0.0, 320.5, 0.0, 1.0, 240.5, 0.0, 0.0, 1.0]
msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
msg.P = [
1.0, 0.0, 320.5, -0.0, 0.0, 1.0, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0
]
msg.binning_x = 0
msg.binning_y = 0
msg.roi = msg_roi
return msg
def getResult(self, predictions):
boxes = predictions.pred_boxes if predictions.has(
"pred_boxes") else None
if predictions.has("pred_masks"):
masks = np.asarray(predictions.pred_masks)
else:
return
result_msg = Result()
result_msg.header = self._header
result_msg.class_ids = predictions.pred_classes if predictions.has(
"pred_classes") else None
result_msg.class_names = np.array(
self._class_names)[result_msg.class_ids.numpy()]
result_msg.scores = predictions.scores if predictions.has(
"scores") else None
for i, (x1, y1, x2, y2) in enumerate(boxes):
mask = np.zeros(masks[i].shape, dtype="uint8")
mask[masks[i, :, :]] = 255
mask = self._bridge.cv2_to_imgmsg(mask)
mask.encoding = "mono8" # 参考mask_rcnn_node line164
result_msg.masks.append(mask)
box = RegionOfInterest()
box.x_offset = np.uint32(x1)
box.y_offset = np.uint32(y1)
box.height = np.uint32(y2 - y1)
box.width = np.uint32(x2 - x1)
result_msg.boxes.append(box)
return result_msg
def test_WithBoundingBox(self):
bbox = RegionOfInterest(30, 40, 210, 302)
species_id = SingleSpeciesId(
bbox,
[SpeciesScore(3, 0.4), SpeciesScore(2, 0.3)])
SpeciesSelectionModule.speciesIdCallback(
SpeciesIDResponse(3, [species_id]), self.module)
self.checkSignalUpdate(3)
d = self.module.GetOutputDict()
self.assertEquals(d['state'], State.CANDIDATES_ARRIVED)
self.assertEquals(d['name'], 'SpeciesSelection')
foundBbox = d['bbox']
self.assertEquals(foundBbox['x_offset'], 30)
self.assertEquals(foundBbox['y_offset'], 40)
self.assertEquals(foundBbox['height'], 210)
self.assertEquals(foundBbox['width'], 302)
self.assertEquals(len(d['species_hits']), 3)
self.checkSpeciesInfo(self.speciesInfo[3], d['species_hits'][0])
self.checkSpeciesInfo(self.speciesInfo[2], d['species_hits'][1])
self.checkSpeciesInfo(SpeciesSelectionModule.UNKNOWN_SPECIES,
d['species_hits'][2])
self.assertEquals(d['user_chose'], False)
self.assertEquals(d['idx'], 0)
def detect_poses(self, image):
# `op` added to globals in the constructor
datum = op.Datum() # pylint: disable=undefined-variable # noqa: F821
datum.cvInputData = image
self._openpose_wrapper.emplaceAndPop(op.VectorDatum([datum]))
keypoints = datum.poseKeypoints
overlayed_image = datum.cvOutputData
recognitions = []
if keypoints is not None and len(
keypoints.shape
) == 3: # If no detections, keypoints will be None
num_persons, num_bodyparts, _ = keypoints.shape
for person_id in range(0, num_persons):
for body_part_id in range(0, num_bodyparts):
body_part = self._model["body_parts"][body_part_id]
x, y, probability = keypoints[person_id][body_part_id]
if probability > 0:
recognitions.append(
Recognition(group_id=person_id,
roi=RegionOfInterest(width=1,
height=1,
x_offset=int(x),
y_offset=int(y)),
categorical_distribution=
CategoricalDistribution(probabilities=[
CategoryProbability(
label=body_part,
probability=float(probability))
])))
return recognitions, overlayed_image
def __init__(self, **kwargs):
assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \
"Invalid arguments passed to constructor: %r" % kwargs.keys()
from std_msgs.msg import Header
self.header = kwargs.get('header', Header())
self.height = kwargs.get('height', int())
self.width = kwargs.get('width', int())
self.distortion_model = kwargs.get('distortion_model', str())
self.d = kwargs.get('d', list())
self.k = kwargs.get(
'k',
list([float() for x in range(9)])
)
self.r = kwargs.get(
'r',
list([float() for x in range(9)])
)
self.p = kwargs.get(
'p',
list([float() for x in range(12)])
)
self.binning_x = kwargs.get('binning_x', int())
self.binning_y = kwargs.get('binning_y', int())
from sensor_msgs.msg import RegionOfInterest
self.roi = kwargs.get('roi', RegionOfInterest())
def imgCallback(self, image):
num = 0
try:
cv_image = self.bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print(e)
# 如果存在人
roi = RegionOfInterest()
try:
position = self.detectWave(cv_image, self.check_gender)
cv2.rectangle(cv_image, (position[1], position[3]),
(position[0], position[2]), (0, 0, 255))
roi.x_offset = position[0]
roi.y_offset = position[2]
roi.width = position[1] - position[0]
roi.height = position[3] - position[2]
self.roi = roi
self.roi_pub.publish(roi)
if self.ispub == False:
stringq = "I can tell one wave. Now I will recognize people. "
self.word_pub.publish(stringq)
stringq = "ok "
self.face_pub.publish(stringq)
self.ispub = True
rospy.loginfo("One Wave!")
num = 1
except:
self.roi = roi
if num == 0:
self.roi_pub.publish(roi)
cv_image = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
self.img_pub.publish(cv_image)
def getResult(predictions, classes):
boxes = predictions.pred_boxes if predictions.has("pred_boxes") else None
if predictions.has("pred_masks"):
masks = np.asarray(predictions.pred_masks)
#print(type(masks))
else:
return
result_msg = Result()
# result_msg.header = self._header
result_msg.class_ids = predictions.pred_classes if predictions.has(
"pred_classes") else None
result_msg.class_names = np.array(classes)[result_msg.class_ids.numpy()]
result_msg.scores = predictions.scores if predictions.has(
"scores") else None
for i, (x1, y1, x2, y2) in enumerate(boxes):
mask = np.zeros(masks[i].shape, dtype="uint8")
mask[masks[i, :, :]] = 255
mask = br.cv2_to_imgmsg(mask)
result_msg.masks.append(mask)
box = RegionOfInterest()
box.x_offset = np.uint32(x1)
box.y_offset = np.uint32(y1)
box.height = np.uint32(y2 - y1)
box.width = np.uint32(x2 - x1)
result_msg.boxes.append(box)
return result_msg
def callback(image_msg):
global pub_Image
global pub_BoxesImage
global pub_Boxes
global face_cascade
global bridge
global boxes_msg
global faces
global local_position_pub
global goal_pose
# Convert ros image into a cv2 image
cv_img = bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough")
cv_gray = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
# Find the faces and draw a rectangle around them
faces = face_cascade.detectMultiScale(cv_gray, 1.3, 5)
for (x, y, w, h) in faces:
cv_img = cv2.rectangle(cv_img, (x, y), (x + w, y + h), (255, 0, 0), 2)
# For each image, we also want to publish the croped image
crop_img = cv_img[y:y + h, x:x + w]
crop_msg = bridge.cv2_to_imgmsg(crop_img, encoding="bgr8")
# And the region of interest information
pub_BoxesImage.publish(crop_msg)
boxes_msg = RegionOfInterest()
boxes_msg.x_offset = x
boxes_msg.y_offset = y
boxes_msg.height = h
boxes_msg.width = w
boxes_msg.do_rectify = False
pub_Boxes.publish(boxes_msg)
# Publish the original image with the rectangles
mod_img = bridge.cv2_to_imgmsg(cv_img, encoding="bgr8")
pub_Image.publish(mod_img)
def mouse_callback(self, event, x, y, flags, param):
print 'Mouse callback status', self.status
if self.status == 'initializing':
if event == cv2.EVENT_MOUSEMOVE:
self.color_image_drawing = np.copy(self.color_image)
p1_x = max(0, x - self.x_width / 2)
p1_y = max(0, y - self.y_width / 2)
p2_x = min(self.img_size_x - 1, x + self.x_width / 2)
p2_y = min(self.img_size_y - 1, y + self.y_width / 2)
cv2.rectangle(self.color_image_drawing, (p1_x, p1_y),
(p2_x, p2_y),
color=(0, 255, 0),
thickness=1)
if event == cv2.EVENT_LBUTTONDOWN:
self.status = 'tracking'
init_track_msg = RegionOfInterest()
init_track_msg.x_offset = max(0, x - self.x_width / 2)
init_track_msg.y_offset = max(0, y - self.y_width / 2)
init_track_msg.height = self.y_width
init_track_msg.width = self.x_width
self.init_tracking_pub.publish(init_track_msg)
def __init__(self):
APP_ID = '18721308'
API_KEY = 'lNQGdBNazTPv8LpSP4x0GQlI'
SECRET_KEY = 'nW8grONY777n4I2KvpOVuKGDNiY03omI'
self.client_face = AipFace(APP_ID, API_KEY, SECRET_KEY)
self.client_body = AipBodyAnalysis(APP_ID, API_KEY, SECRET_KEY)
self.image_type = "BASE64"
filepath = "/Pictures/image.jpg"
msg = reception_image()
self.guest = Guest()
# self.name = None
# self.age = None
# self.image = None
# self.gender = None
# self.drink = None
self.options_body = {}
self.options_body["type"] = "gender,upper_color,upper_wear_fg"
self.options_face = {}
self.options_face["face_field"] = "age,gender"
self.options_face["max_face_num"] = 3
self.options_face["face_type"] = "LIVE"
#发布器
self.roi_pub = rospy.Publisher('/image/roi',RegionOfInterest,queue_size=1)
self.objpos_pub = rospy.Publisher('/image/object_position2d',String,queue_size=1)
self.control_pub = rospy.Pubscriber("/control", reception, queue_size=1)
self.speech_pub = rospy.Pubscriber('/speech/check_door',reception_image,self.find_people)
#订阅器
self.control_sub = rospy.Subscriber("/control", reception, self.controlCallback)
self.ROI = RegionOfInterest()
def _build_result_msg(self, msg, result):
result_msg = Result()
result_msg.header = msg.header
for i, (y1, x1, y2, x2) in enumerate(result['rois']):
box = RegionOfInterest()
box.x_offset = x1#np.asscalar(x1)
box.y_offset = y1#np.asscalar(y1)
box.height = y2-y1#np.asscalar(y2 - y1)
box.width = x2-x1#np.asscalar(x2 - x1)
result_msg.boxes.append(box)
class_id = result['class_ids'][i]
result_msg.class_ids.append(class_id)
class_name = self._class_names[class_id]
result_msg.class_names.append(class_name)
score = result['scores'][i]
result_msg.scores.append(score)
mask = Image()
mask.header = msg.header
mask.height = result['masks'].shape[0]
mask.width = result['masks'].shape[1]
mask.encoding = "mono8"
mask.is_bigendian = False
mask.step = mask.width
mask.data = (result['masks'][:, :, i] * 255).tobytes()
result_msg.masks.append(mask)
return result_msg
def convert_to_ros_bounding_box(bounding_box):
ros_bb = RegionOfInterest()
ros_bb.x_offset = bounding_box['x']
ros_bb.y_offset = bounding_box['y']
ros_bb.width = bounding_box['width']
ros_bb.height = bounding_box['height']
return ros_bb
def __init__(self):
self.node_name = "crop_row_publisher"
rospy.init_node(self.node_name)
# What we do during shutdown
rospy.on_shutdown(self.cleanup)
# Create the OpenCV display window for the RGB image
self.cv_window_name = self.node_name
# Initialize the Region of Interest and its publisher
self.ROI = RegionOfInterest()
self.roi_pub = rospy.Publisher("/roi", RegionOfInterest, queue_size=1)
# Create the cv_bridge object
self.bridge = CvBridge()
# Subscribe to the camera image and depth topics and set
# the appropriate callbacks
#self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback, queue_size=1)
self.image_sub = rospy.Subscriber("/camera/rgb/image_raw",
Image,
self.image_callback,
queue_size=1)
rospy.loginfo("Waiting for image topics...")
rospy.wait_for_message("input_rgb_image", Image)
rospy.loginfo("Ready.")
def _generate_dummy_person3d(self, rgb, depth, cam_info, name=None):
person = Person3D()
person.header = rgb.header
person.name = name if name else "" # Empty name makes this person unknwown
person.age = 20 + self._counter
person.gender = random.choice([0, 1])
person.gender_confidence = random.normalvariate(mu=0.75, sigma=0.2)
person.posture = random.choice(['standing', 'sitting'])
person.emotion = 'happy'
colors = ['black', 'orange', 'yellow']
random.shuffle(colors)
person.shirt_colors = colors
person.tags = ['LWave', 'RWave'] + random.choice([[], ["is_pointing"]])
# person.body_parts_pose
person.position = Point()
person.face.roi = RegionOfInterest(width=200, height=200)
xs = [-2, -1, -0.5, 0.5, 1, 2]
random.shuffle(xs)
person.position.x = xs.pop()
zs = [0.5, 1, 2, 3]
random.shuffle(zs)
person.position.z = zs.pop()
person.pointing_pose = Pose(
position=person.position,
orientation=Quaternion(*quaternion_from_euler(0, 0, math.pi / 2)))
return person
def format_msg(src, Obj, s=2):
assert type(Obj)==type and 'msg' in str(Obj)
from sensor_msgs.msg import RegionOfInterest
from geometry_msgs.msg import Point32, Polygon
hd = hasattr(src,'header')
if type(src)==str: im = cv2.imread(src)
elif type(src)==np.ndarray: im = src.copy()
elif hd: im = CVB.compressed_imgmsg_to_cv2(src)
obj = Obj(header=src.header) if hd else Obj()
im, res = get_instances(im, HSV_Set, {}, s)
if not res: return obj, im, res
if hasattr(Obj,'class_ids'): obj.class_ids = res['cid']
if hasattr(Obj,'class_names'): obj.class_names = res['cls']
if hasattr(Obj,'scores'): obj.scores = [1.0]*len(res['cls'])
if hasattr(Obj,'boxes'): obj.boxes = [RegionOfInterest(x_offset=x,
y_offset=y, width=w, height=h) for (x,y,w,h) in res['box']]
if hasattr(Obj,'contours'): obj.contours = [Polygon([Point32(
x=x, y=y) for (x,y) in c.transpose()]) for c in res['cnt']]
elif hasattr(Obj,'masks'): # for masks
for mk in res['mask']: # get masks
m = CVB.cv2_to_imgmsg(mk.astype('uint8')*255, 'mono8')
m.header = obj.header; obj.masks.append(m)
return obj, im, res
def _get_face_rois_ids_openpose(recognitions):
"""
Get ROIs of faces from openpose recognitions using the nose, left ear
and right ear
:param: recognitions from openpose
"""
nose_recognitions = PeopleRecognizer2D._get_recognitions_with_label(
"Nose", recognitions)
left_ear_recognitions = PeopleRecognizer2D._get_recognitions_with_label(
"LEar", recognitions)
right_ear_recognitions = PeopleRecognizer2D._get_recognitions_with_label(
"REar", recognitions)
rois = []
group_ids = []
for nose_recognition in nose_recognitions:
# We assume a vertical head here
left_size = 50
right_size = 50
try:
left_ear_recognition = next(
r for r in left_ear_recognitions
if r.group_id == nose_recognition.group_id)
left_size = math.hypot(
left_ear_recognition.roi.x_offset -
nose_recognition.roi.x_offset,
left_ear_recognition.roi.y_offset -
nose_recognition.roi.y_offset)
except StopIteration:
pass
try:
right_ear_recognition = next(
r for r in right_ear_recognitions
if r.group_id == nose_recognition.group_id)
right_size = math.hypot(
right_ear_recognition.roi.x_offset -
nose_recognition.roi.x_offset,
right_ear_recognition.roi.y_offset -
nose_recognition.roi.y_offset)
except StopIteration:
pass
size = left_size + right_size
width = int(size)
height = int(math.sqrt(2) * size)
rois.append(
RegionOfInterest(x_offset=max(
0, int(nose_recognition.roi.x_offset - .5 * width)),
y_offset=max(
0,
int(nose_recognition.roi.y_offset -
.5 * height)),
width=width,
height=height))
group_ids.append(nose_recognition.group_id)
return rois, group_ids
def main():
rospy.wait_for_service('detect_keypoints')
detect_keypoint = rospy.ServiceProxy('detect_keypoints',
MankeyKeypointDetection)
# Get the test data path
project_path = os.path.join(os.path.dirname(__file__), os.path.pardir)
project_path = os.path.abspath(project_path)
test_data_path = os.path.join(project_path, 'test_data')
cv_rbg_path = os.path.join(test_data_path, '000000_rgb.png')
cv_depth_path = os.path.join(test_data_path, '000000_depth.png')
# Read the image
cv_rgb = cv2.imread(cv_rbg_path, cv2.IMREAD_COLOR)
cv_depth = cv2.imread(cv_depth_path, cv2.IMREAD_ANYDEPTH)
# The bounding box
roi = RegionOfInterest()
roi.x_offset = 261
roi.y_offset = 194
roi.width = 327 - 261
roi.height = 260 - 194
# Build the request
request = MankeyKeypointDetectionRequest()
bridge = CvBridge()
request.rgb_image = bridge.cv2_to_imgmsg(cv_rgb, 'bgr8')
request.depth_image = bridge.cv2_to_imgmsg(cv_depth)
request.bounding_box = roi
response = detect_keypoint(request)
print response
def __init__(self):
self.bridge = CvBridge()
self.rospack = rospkg.RosPack()
self.roi = RegionOfInterest()
rospy.on_shutdown(self.shutdown)
# initialize dlib's face detector (HOG-based) and then create
# the facial landmark predictor
self.p = os.path.sep.join(
[self.rospack.get_path('common_face_application')])
self.libraryDir = os.path.join(self.p, "library")
self.dlib_filename = self.libraryDir + "/shape_predictor_68_face_landmarks.dat"
self.detector = dlib.get_frontal_face_detector()
self.predictor = dlib.shape_predictor(self.dlib_filename)
# Subscribe to Image msg
image_topic = "/cv_camera/image_raw"
self.image_sub = rospy.Subscriber(image_topic, Image, self.cbImage)
# Subscribe to CameraInfo msg
cameraInfo_topic = "/cv_camera/camera_info"
self.cameraInfo_sub = rospy.Subscriber(cameraInfo_topic, CameraInfo,
self.cbCameraInfo)
# Publish to RegionOfInterest msgs
roi_topic = "/faceROI"
self.roi_pub = rospy.Publisher(roi_topic,
RegionOfInterest,
queue_size=10)
def detect_poses(self, image):
keypoints, overlayed_image = self._openpose.forward(image, True)
recognitions = []
if len(keypoints.shape) == 3:
num_persons, num_bodyparts, _ = keypoints.shape
for person_id in range(0, num_persons):
for body_part_id in range(0, num_bodyparts):
body_part = self._model["body_parts"][body_part_id]
x, y, probability = keypoints[person_id][body_part_id]
if probability > 0:
recognitions.append(
Recognition(group_id=person_id,
roi=RegionOfInterest(width=1,
height=1,
x_offset=int(x),
y_offset=int(y)),
categorical_distribution=
CategoricalDistribution(probabilities=[
CategoryProbability(
label=body_part,
probability=float(probability))
])))
return recognitions, overlayed_image
def publish_roi(self):
msg = RegionOfInterest()
msg.x_offset = (self.center[0] - self.majorAxis) * self.overlayImgSize
msg.y_offset = (self.center[1] - self.minorAxis) * self.overlayImgSize
msg.width = int(self.majorAxis * 2 * self.overlayImgSize)
msg.height = int(self.minorAxis * 2 * self.overlayImgSize)
self.roiPub.publish(msg)
def __init__(self, **kwargs):
assert all('_' + key in self.__slots__ for key in kwargs.keys()), \
'Invalid arguments passed to constructor: %s' % \
', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__))
from std_msgs.msg import Header
self.header = kwargs.get('header', Header())
self.height = kwargs.get('height', int())
self.width = kwargs.get('width', int())
self.distortion_model = kwargs.get('distortion_model', str())
self.d = array.array('d', kwargs.get('d', []))
if 'k' not in kwargs:
self.k = numpy.zeros(9, dtype=numpy.float64)
else:
self.k = numpy.array(kwargs.get('k'), dtype=numpy.float64)
assert self.k.shape == (9, )
if 'r' not in kwargs:
self.r = numpy.zeros(9, dtype=numpy.float64)
else:
self.r = numpy.array(kwargs.get('r'), dtype=numpy.float64)
assert self.r.shape == (9, )
if 'p' not in kwargs:
self.p = numpy.zeros(12, dtype=numpy.float64)
else:
self.p = numpy.array(kwargs.get('p'), dtype=numpy.float64)
assert self.p.shape == (12, )
self.binning_x = kwargs.get('binning_x', int())
self.binning_y = kwargs.get('binning_y', int())
from sensor_msgs.msg import RegionOfInterest
self.roi = kwargs.get('roi', RegionOfInterest())
def publish(self, boxes, scores, classes, num, image_shape, category_index, masks=None):
# init detection message
msg = Detection()
for i in range(boxes.shape[0]):
if scores[i] > 0.5:
ymin, xmin, ymax, xmax = tuple(boxes[i].tolist())
box = RegionOfInterest()
box.x_offset = np.asscalar(xmin)
box.y_offset = np.asscalar(ymin)
box.height = np.asscalar(ymax - ymin)
box.width = np.asscalar(xmax - xmin)
display_str = '##\nnumber {} {}: {}% at image coordinates (({}, {}) to ({}, {}))\n##'.format(i, class_name, int(100*scores[i]), left, top, right, bottom)
print(display_str)
# fill detection message with objects
obj = Object()
obj.box = box
obj.class_name = class_name
obj.score = int(scores[i])
obj.mask = masks[i]
msg.objects.append(obj)
# publish detection message
self.DetPub.publish(msg)
def publish_roi(self):
if not self.drag_start:
if self.track_box is not None:
roi_box = self.track_box
elif self.detect_box is not None:
roi_box = self.detect_box
else:
return
try:
roi_box = self.cvBox2D_to_cvRect(roi_box)
except:
return
# Watch out for negative offsets
roi_box[0] = max(0, roi_box[0])
roi_box[1] = max(0, roi_box[1])
try:
ROI = RegionOfInterest()
ROI.x_offset = int(roi_box[0])
ROI.y_offset = int(roi_box[1])
ROI.width = int(roi_box[2])
ROI.height = int(roi_box[3])
self.roi_pub.publish(ROI)
except:
rospy.loginfo("Publishing ROI failed")
def __init__(self):
rospy.init_node('marker_detection', anonymous=True)
self.image_pub = rospy.Publisher("/detection/image_raw",
Image,
queue_size=1)
# Initialize the Region of Interest and its publisher
self.ROI = RegionOfInterest()
self.ROI_pub = rospy.Publisher("/ch1/marker_bb",
RegionOfInterest,
queue_size=1)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/downward_cam/camera/image",
Image,
self.callback,
queue_size=1,
buff_size=2**24,
tcp_nodelay=True)
self.image_width = 1
self.image_height = 1
self.cameraInfo_sub = rospy.Subscriber(
"/downward_cam/camera/camera_info",
CameraInfo,
self.get_camera_info,
queue_size=1)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
rate.sleep()
def face_detect_cb(self, event):
if self.image is not None and self.lock.acquire(False):
cnn_results = self.recogniser.detect_faces(self.image,
scale=self.cnn_scale)
features = Features()
features.features = []
for k, d in enumerate(cnn_results):
padding = int(self.image.shape[0] * self.cnn_padding)
feature = Feature()
roi = RegionOfInterest()
roi.x_offset = np.maximum(d.left() - padding, 0)
roi.y_offset = np.maximum(d.top() - padding, 0)
roi.height = np.minimum(d.bottom() - roi.y_offset + padding,
self.image.shape[0])
roi.width = np.minimum(d.right() - roi.x_offset + padding,
self.image.shape[1])
feature.roi = roi
feature.crop = self.bridge.cv2_to_imgmsg(
np.array(
self.image[roi.y_offset:roi.y_offset + roi.height,
roi.x_offset:roi.x_offset + roi.width, :]))
features.features.append(feature)
self.lock.release()
self.faces_pub.publish(features)
def publish(self,
boxes,
scores,
classes,
num,
category_index,
masks=None,
fps=0):
# init detection message
msg = Detection()
for i in range(boxes.shape[0]):
if scores[i] > 0.5:
if classes[i] in category_index.keys():
class_name = category_index[classes[i]]['name']
else:
class_name = 'N/A'
ymin, xmin, ymax, xmax = tuple(boxes[i].tolist())
box = RegionOfInterest()
box.x_offset = xmin + (xmax - xmin) / 2.0
box.y_offset = ymin + (ymax - ymin) / 2.0
box.height = ymax - ymin
box.width = xmax - xmin
# fill detection message with objects
obj = Object()
obj.box = box
obj.class_name = class_name
obj.score = int(100 * scores[i])
if masks is not None:
obj.mask = self._bridge.cv2_to_imgmsg(
masks[i], encoding="passthrough")
msg.objects.append(obj)
msg.fps = fps
# publish detection message
self.DetPub.publish(msg)
