def twist_received(self, twist):
"""
receive twist and convert to carla vehicle control
"""
control = CarlaEgoVehicleControl()
if twist == Twist():
#stop
control.throttle = 0.
control.brake = 1.
control.steer = 0.
else:
if twist.linear.x > 0:
control.throttle = min(TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x)/ \
TwistToVehicleControl.MAX_LON_ACCELERATION
else:
control.reverse = True
control.throttle = max(-TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x)/ \
-TwistToVehicleControl.MAX_LON_ACCELERATION
if twist.angular.z > 0:
control.steer = -min(self.max_steering_angle, twist.angular.z)/ \
self.max_steering_angle
else:
control.steer = -max(-self.max_steering_angle, twist.angular.z)/ \
self.max_steering_angle
#invert steer for reverse case
if control.reverse:
control.steer = -control.steer
self.pub.publish(control)
def twist_received(self, twist):
"""
receive twist and convert to carla vehicle control
"""
control = CarlaEgoVehicleControl()
if twist == Twist():
# stop
control.throttle = 0.
control.brake = 1.
control.steer = 0.
else:
if twist.linear.x > 0:
control.throttle = min(
TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x
) / TwistToVehicleControl.MAX_LON_ACCELERATION
else:
control.reverse = True
control.throttle = max(
-TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x
) / -TwistToVehicleControl.MAX_LON_ACCELERATION
if twist.angular.z > 0:
control.steer = -min(self.max_steering_angle, twist.angular.z) / \
self.max_steering_angle
else:
control.steer = -max(-self.max_steering_angle, twist.angular.z) / \
self.max_steering_angle
try:
self.pub.publish(control)
except rospy.ROSException as e:
if not rospy.is_shutdown():
rospy.logwarn("Error while publishing control: {}".format(e))
def control_callback(msg):
data = CarlaEgoVehicleControl()
data.throttle = msg.velocity
data.steer = msg.angle
data.brake = 0
data.gear = 1
data.reverse = False
data.manual_gear_shift = True
pub.publish(data)
vehicle_control_manual_override_publisher.publish(vehicle_control_manual_override)
auto_pilot_enable_publisher.publish(autopilot_enabled)
def main():
ack = Ani()
rospy.init_node('Carla_control')
rate = rospy.Rate(10)
while not rospy.is_shutdown():
cmd_msg = AckermannDrive()
msg = CarlaEgoVehicleControl()
cmd_msg.speed = ack.speed
cmd_msg.steering_angle = ack.steering_angle
msg.steer = cmd_msg.steering_angle
msg.throttle = cmd_msg.speed
if ack.brake == 1:
if cmd_msg.speed < 0:
msg.reverse = 15
if ack.brake == -1:
msg.brake = True
ack.pub_callback(msg)
rate.sleep()
def Control(self):
rate = rospy.Rate(10)
lon_param = {
'K_P': 0.5,
'K_I': 0.5,
'K_D': 0
} # Set PID values for longitudinal controller
lat_param = {
'K_P': 0.5,
'K_I': 0.3,
'K_D': 0
} # Set PID values for lateral controller
vehicle_controller = VehiclePIDController(
self.vehicle, lon_param,
lat_param) # Calling vehicle controller class from controller.py
i = 0
for k in range(1, len(self.current_route)
): # Iterate through all the waypoints in the route
self.Distance(self.current_route[i][0], self.veh_pos.transform)
self.Waypoints()
rospy.Subscriber(
'/machine_learning/output', Int16, self.Detection
) # Subscribes to topic for Stop sign detection. Need to run carla_detect_objects.py script to obtain detection
while self.distance > 0.5: # Control the vehicle until the distance of the next waypoint and the vehicle is less than 0.5 m
self.Actor(
) # Call Actor function to update vehicle's location
control = vehicle_controller.run_step(
15, self.current_route[i]
[0]) # Feeds the controller the waypoints one by one
self.velocity = self.vehicle.get_velocity(
) # Get vehicle velocity
if self.detection == 11: # Stop sign detection(apply brakes). Our ML has a class ID of 11 for stop signs
print('Object detected, apply brakes')
msg = CarlaEgoVehicleControl(
) # Ego vehicle's control message
msg.throttle = 0
msg.steer = control.steer
msg.brake = 1
msg.hand_brake = control.hand_brake
msg.reverse = control.reverse
msg.gear = 1
msg.manual_gear_shift = control.manual_gear_shift
self.detection = None
elif len(self.current_route) - 5 <= k <= len(
self.current_route
): # If the ith waypoint is between the last waypoint minus five apply brakes
msg = CarlaEgoVehicleControl()
msg.throttle = 0
msg.steer = control.steer
msg.brake = 1
msg.hand_brake = control.hand_brake
msg.reverse = control.reverse
msg.gear = 1
msg.manual_gear_shift = control.manual_gear_shift
print('You arrived to your destination!!')
else: # If neither scenario happen, keep driving
msg = CarlaEgoVehicleControl()
msg.throttle = control.throttle
msg.steer = control.steer
msg.brake = control.brake
msg.hand_brake = control.hand_brake
msg.reverse = control.reverse
msg.gear = 1
msg.manual_gear_shift = control.manual_gear_shift
self.Publisher(msg)
rate.sleep()
self.Distance(
self.current_route[i][0], self.veh_pos.transform
) # Calculates the Euclidean distance between the vehicle and the next waypoint in every iteration
i += 1
