class FaceToFace():
def __init__(self):
sub = rospy.Subscriber("/cv_camera/image_raw", Image, self.get_image)
self.bridge = CvBridge()
self.image_org = None
self.pub = rospy.Publisher("face", Image, queue_size=1)
###モータの制御に関する処理を追加###
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rospy/wait_for_service('/motor_on')
rospy.wait_for_service('/motor_off')
rospy.on_shutdown(rospy.ServiceProxy('/motor_off', Trigger).call)
rospy.ServiceProxy('/motor_on', Trigger).call()
def monitor(self,rect,org):
if rect is not None:
cv2.rectangle(org,tuple(rect[0:2]),tuple(rect[0:2]+rect[2:4]),(0,255,255),4)
self.pub.publish(self.bridge.cv2_to_imgsg(org, "bgr8"))
def get_image(self,img):
try:
self.image_org = self.bridge.imgmsg_to_cv2(img, "bgr8")
expect CvBridgeError as e:
rospy.logerr(e)
def rot_vel(self): #このメソッドを追加
r = self.detect_face()
if r is None:
return 0.0
wid = self.image_org.shape[1]/2 #画像の幅の半分の値
pos_x_rate = (r[0] + r[2]/2 - wid)*1.0*wid
rot = 0.25*pos_x_rate*math.pi #画面のキワに顔がある場合にpi/4[rad/s]に
rospy.loginfo("detected %f",rot)
return rot
def control(self):
m = Twist()
m.linear.x = 0.0
m.angular.z = self.rot_vel()
self.cmd_vel.publish(m)
if __neme__ == '__main__':
rospy.init_node('face_to_face')
fd = FaceToFace()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
fd.control()
rate.sleep()
