Skip to content

Instantly share code, notes, and snippets.

@jtbandes

jtbandes/step1.py Secret

Last active Jul 16, 2021
Embed
What would you like to do?
# Step 1: a basic ROS node that receives camera images.
import rospy
from sensor_msgs.msg import Image
def image_callback(msg: Image):
rospy.loginfo("Got an image!")
def main():
rospy.init_node("my_node")
rospy.Subscriber("/usb_cam/image_raw", Image, image_callback, queue_size=1)
rospy.spin()
if __name__ == "__main__":
main()
# Step 2: run the dlib face detector and log the number of faces found.
import cv2
import rospy
import dlib
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
cv_bridge = CvBridge()
face_detector = dlib.get_frontal_face_detector()
def image_callback(msg: Image):
# Convert the ROS Image to a grayscale OpenCV image
cv_img = cv_bridge.imgmsg_to_cv2(msg)
grayscale_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
# Run the face detector on the grayscale image
rects = face_detector(grayscale_img, 0)
rospy.loginfo(f"Detected {len(rects)} faces")
def main():
rospy.init_node("my_node")
rospy.Subscriber("/usb_cam/image_raw", Image, image_callback, queue_size=1)
rospy.spin()
if __name__ == "__main__":
main()
# Step 3: publish an ImageMarkerArray topic to draw a box around each face.
import cv2
import rospy
import dlib
from cv_bridge import CvBridge
from std_msgs.msg import ColorRGBA
from sensor_msgs.msg import Image
from foxglove_msgs.msg import ImageMarkerArray
from visualization_msgs.msg import ImageMarker
from geometry_msgs.msg import Point
cv_bridge = CvBridge()
face_detector = dlib.get_frontal_face_detector()
class Node:
pub_markers: rospy.Publisher
def __init__(self):
rospy.init_node("my_node")
rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback, queue_size=1)
self.pub_markers = rospy.Publisher(
"/usb_cam/face_markers", ImageMarkerArray, queue_size=1
)
rospy.spin()
def image_callback(self, msg: Image):
# Convert the ROS Image to a grayscale OpenCV image
cv_img = cv_bridge.imgmsg_to_cv2(msg)
grayscale_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
# Run the face detector on the grayscale image
rects = face_detector(grayscale_img, 0)
rospy.loginfo(f"Detected {len(rects)} faces")
# Display each face as a cyan rectangle
markers = ImageMarkerArray()
for rect in rects:
markers.markers.append(
ImageMarker(
header=msg.header,
type=ImageMarker.LINE_STRIP,
outline_color=ColorRGBA(0, 255, 255, 1),
points=[
Point(rect.left(), rect.top(), 0),
Point(rect.right(), rect.top(), 0),
Point(rect.right(), rect.bottom(), 0),
Point(rect.left(), rect.bottom(), 0),
Point(rect.left(), rect.top(), 0),
],
)
)
self.pub_markers.publish(markers)
def main():
Node()
if __name__ == "__main__":
main()
# Step 4: run the facial landmark predictor and draw a polygon for each facial feature.
import cv2
import rospy
import dlib
from imutils.face_utils import FACIAL_LANDMARKS_68_IDXS
from cv_bridge import CvBridge
from std_msgs.msg import ColorRGBA
from sensor_msgs.msg import Image
from foxglove_msgs.msg import ImageMarkerArray
from visualization_msgs.msg import ImageMarker
from geometry_msgs.msg import Point
cv_bridge = CvBridge()
face_detector = dlib.get_frontal_face_detector()
predictor = dlib.shape_predictor("shape_predictor_68_face_landmarks.dat")
COLORS = [
ColorRGBA(0, 0, 0, 1),
ColorRGBA(0, 0, 255, 1),
ColorRGBA(0, 255, 0, 1),
ColorRGBA(0, 255, 255, 1),
ColorRGBA(255, 0, 0, 1),
ColorRGBA(255, 0, 255, 1),
ColorRGBA(255, 255, 0, 1),
ColorRGBA(255, 255, 255, 1),
]
class Node:
pub_markers: rospy.Publisher
def __init__(self):
rospy.init_node("my_node")
rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback, queue_size=1)
self.pub_markers = rospy.Publisher(
"/usb_cam/face_markers", ImageMarkerArray, queue_size=1
)
rospy.spin()
def image_callback(self, msg: Image):
# Convert the ROS Image to a grayscale OpenCV image
cv_img = cv_bridge.imgmsg_to_cv2(msg)
grayscale_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
# Run the face detector on the grayscale image
rects = face_detector(grayscale_img, 0)
rospy.loginfo(f"Detected {len(rects)} faces")
markers = ImageMarkerArray()
for rect in rects:
# Run the predictor, which returns a list of 68 facial landmarks as (x,y) points
points = predictor(grayscale_img, rect).parts()
# Draw a line around each face region
for region_idx, (name, (start_idx, end_idx)) in enumerate(
FACIAL_LANDMARKS_68_IDXS.items()
):
region_points = points[start_idx:end_idx]
# Connect the points for each region in a loop, except for the jaw
if name != "jaw":
region_points.append(region_points[0])
markers.markers.append(
ImageMarker(
header=msg.header,
type=ImageMarker.LINE_STRIP,
points=[Point(p.x, p.y, 0) for p in region_points],
outline_color=COLORS[region_idx % len(COLORS)],
)
)
self.pub_markers.publish(markers)
def main():
Node()
if __name__ == "__main__":
main()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment