sitl_file.py 文件源码

def __init__(self):
      self.imgprocess = ImageProcessor.ImageProcessor()
      self.bridge = CvBridge()
      self.latestimage = None
      self.process = False

      """ROS Subscriptions """
      self.image_pub = rospy.Publisher("/image_converter/output_video",Image, queue_size=10)
      self.image_sub = rospy.Subscriber("/cam/camera_/image_raw",Image,self.cvt_image)
      self.cmdVelocityPub = rospy.Publisher('/platform_control/cmd_vel', Twist, queue_size=10)

      self.targetlane = 0
      self.cmdvel = Twist()
      self.last_time = rospy.Time()
      self.sim_time = rospy.Time()
      self.dt = 0
      self.position_er = 0
      self.position_er_last = 0;
      self.cp = 0
      self.vel_er = 0
      self.cd = 0
      self.Kp = 3
      self.Kd = 3.5