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()