# encoding: utf-8
__author__ = 'Gui'
'''
@Time:2022年6月5日 18:15
@Auth:19级机器人工程 (按拼音排序) 桂源泽 苏琦 颜欢
@File:fira_robot_control.py
@IDE:fira项目键盘控制程序
@Software: PyCharm
'''

import rospy
import cv2
from robot import Robot
from geometry_msgs.msg import Twist

rate = 10 # 向ros发布命令频率
v = 0.1 # 前进距离
angular = 0.1 # 旋转角度

class ControlRobot(object):
    """
    input:
        commands
    output:
        小车底盘控制消息
    """
    def __init__(self):      
        #控制底盘
        self.robot = Robot()
        
        # 发布话题相关参数
        self.rate_run = rospy.Rate(rate)
        self.twist = Twist()

        """
        cmd:
        w:前进
        a:左转
        d:右转
        s:后退
        i:左自转
        j:右自转
        l:停止
        """
        self.control()#开始控制小车


    def control(self):
        print("prepar to control.............")
        frame = self.robot.get_image()

        while True:
            frame = self.robot.get_image()
            cv2.imshow("control img", frame)
            command = cv2.waitKey(100) & 0xFF # 等待输入按键取后八位
            command = input("enter command:")
            print("get key:",command)
            if command == 255:
                self.twist.linear.x = 0
                self.twist.angular.z = 0
                continue

            elif command == ord('q'):
                print("..............quiting.............")
                break

            # forward -- 0
            elif command == ord('w'):
                self.twist.linear.x = v
                self.twist.angular.z = 0
                print("前进")

            # forward-left -- 1
            elif command == ord('a'):
                self.twist.linear.x = v
                self.twist.angular.z = angular
                print("左转")

            # forward-right -- 2
            elif command == ord('d'):
                self.twist.linear.x = v
                self.twist.angular.z = -angular
                print("右转")

            # stop-sign -- 3
            elif command == ord('s'):
                self.twist.linear.x = -v
                self.twist.angular.z = 0
                print("后退")

            # road banner front
            elif command == ord('i'):
                self.twist.linear.x = 0.1*v
                self.twist.angular.z = angular
                print("左自转")

            # road banner left
            elif command == ord('j'):
                self.twist.linear.x = 0.1*v
                self.twist.angular.z = -angular
                print("右自转")

            # road banner right
            elif command == ord('l'):
                self.twist.linear.x = 0
                self.twist.angular.z = 0
                print("停止")

            # ‘z’和‘c’用于辅助收集停止符号数据，机器走到停止符号前，左右平移，调整终止符号的位置再拍照，以获得更丰富的数据样本
            elif command == ord('z'):
                self.twist.linear.x = v
                self.twist.angular.z = angular
                continue

            elif command == ord('c'):
                self.twist.linear.x = v
                self.twist.angular.z = -angular
                continue

            # 向机器人底盘发布数据
            self.robot.publish_twist(self.twist)
            self.rate_run.sleep()



if __name__ == '__main__':
    try:
        ControlRobot()
    except KeyboardInterrupt:
        cv2.destroyAllWindows()
        pass