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
