#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge

class LineFollower:
    def __init__(self):
        rospy.init_node('line_follower')
        self.bridge = CvBridge()
        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.image_sub = rospy.Subscriber('/usb_cam/image_raw', Image, self.image_cb)
        # IPM 变换矩阵（需根据标定与安装角度实测填写）
        self.M = cv2.getPerspectiveTransform(self.src_pts, self.dst_pts)
        # PID 参数
        self.kp = 0.005; self.ki = 0.0; self.kd = 0.002
        self.last_err = 0.0
        rospy.spin()

    def image_cb(self, msg):
        frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
        # 1. 逆透视变换
        bird = cv2.warpPerspective(frame, self.M, (frame.shape[1], frame.shape[0]))
        # 2. HSV 白线提取
        hsv = cv2.cvtColor(bird, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv, (0,0,180), (180,60,255))
        # 3. 形态学去噪
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((5,5),np.uint8))
        # 4. 列投影直方图找右路中线
        col_sum = np.sum(mask[mask.shape[0]//2:,:], axis=0)
        # 取最右侧峰作为跟踪目标
        right_peak = self.find_rightmost_peak(col_sum)
        # 5. 计算偏移量并 PID
        err = right_peak - bird.shape[1]/2
        angular_z = -(self.kp*err + self.kd*(err-self.last_err))
        self.last_err = err
        # 6. 发布速度
        twist = Twist()
        twist.linear.x = 0.15
        twist.angular.z = np.clip(angular_z, -0.6, 0.6)
        self.cmd_pub.publish(twist)

if __name__ == '__main__':
    LineFollower()