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