def send_controls(angle, control, us=0.0, seen=False, throttle=0.0): groundSteeringRequest = opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_GroundSteeringRequest( ) groundSteeringRequest.groundSteering = math.radians(angle) if lateral_control_enabled: if debug: print "Steering: %.1f" % angle session.send(1090, groundSteeringRequest.SerializeToString()) pedalPositionRequest = opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_PedalPositionRequest( ) pedalPositionRequest.position = us if longitudinal_control_enabled and (seen or us < 1.4): session.send(1086, pedalPositionRequest.SerializeToString())
maximumDegree = 0.2 if angle > 0 and angle > maximumDegree: angle = maximumDegree if angle < 0 and angle < -maximumDegree: angle = -maximumDegree if distances["front"] < 0.05: angle = 0.00 """ cv2.putText(res,'Angle='+str(angle),(200,20),font,1,(255,255,255),2,cv2.LINE_AA) cv2.imshow("image", res); cv2.waitKey(2);""" groundSteeringRequest = opendlv_standard_message_set_v0_9_6_pb2.opendlv_proxy_GroundSteeringRequest( ) #groundSteeringRequest.groundSteering=0 #error=(numpy.absolute(angle)/maximumDegree) if noBlue + noYellow > 1: error = 0.7 else: error = 0.5 if seeingOrange == 1: error = 0.01 seeingOrange = 0 groundSteeringRequest.groundSteering = error * (0.7 * angle + 0.3 * prevAngle) prevAngle = angle