def run(self):

        spd_data = self.spd_profile.input_file()
        test_data = TestData(self.total_duration, self.sampling_period, spd_data)

        print('\nStarting test:\n\n' + 'Time(s) current_vel(m/s) setpoint_vel(m/s) throttle(%) pid_demand')
        time.sleep(1.25)
        print('.................................................................\n')
        time.sleep(.25)

        p = PID(
                self.K['Kp'], 
                self.K['Ki'],
                self.K['Kd']
                )
        p.setPoint(self.input_sp)
        if self.windup_values is not None:
            p.setWindup(*self.windup_values)
        # self.get_sp_from_data(spd_data)

        # sys.exit() # debug

        start_time = time.time()
        
        self.control_loop(p, start_time, spd_data, test_data)
Example #2
0
 def __init__(self):
   body_id = sml_setup.Get_Parameter(NODE_NAME,'body_id',8)
   body_array = sml_setup.Get_Parameter(NODE_NAME,'body_array',[1,2])
   self.PID = PID()
   self.avoidance = AvoidanceController(body_id,body_array)
   rospy.init_node(NODE_NAME)
   self.N_yaw = sml_setup.Get_Parameter(NODE_NAME,"PID_N_yaw",500)
   self.K_yaw = sml_setup.Get_Parameter(NODE_NAME,"PID_K_yaw",2)
   self.w_inf = sml_setup.Get_Parameter(NODE_NAME,"PID_w_inf",5)
   self.Ktt = sml_setup.Get_Parameter(NODE_NAME,"PID_Ktt",1000)/(20*math.pi/180)
   self.Kphi = sml_setup.Get_Parameter(NODE_NAME,"PID_Kphi",1000)/(20*math.pi/180)
   self.CONTROL_MIN = sml_setup.Get_Parameter(NODE_NAME,"PID_CONTROL_MIN",1000)
   self.CONTROL_NEUTRAL = sml_setup.Get_Parameter(NODE_NAME,"PID_CONTROL_NEUTRAL",1500)
   self.CONTROL_MAX = sml_setup.Get_Parameter(NODE_NAME,"PID_CONTROL_MAX",2000)
   self.CONTROL_ARMING_MIN = sml_setup.Get_Parameter(NODE_NAME,"PID_CONTROL_ARMING_MIN",1025)
   self.CONTROL_CANCEL_GRAVITY = sml_setup.Get_Parameter(NODE_NAME,"PID_CONTROL_CANCEL_GRAVITY",1400)
   self.obstacle_avoidance = sml_setup.Get_Parameter(NODE_NAME,"obstacle_avoidance","False")
Example #3
0
        CarMove.__init__(self)
        CarUltrasound.__init__(self)
        CarInfrared.__init__(self)
        CarCamera.__init__(self)
        CarDetect.__init__(self)

    def AllStop(self):
        CarMove.MotorStop(self)
        CarCamera.CameraCleanup(self)
        GPIO.cleanup()


if __name__ == '__main__':
    try:
        car = Car()
        pid = PID(kp=1, ki=0.0, kd=0.1)
        pid.SetExpectedOutput(15)

        dist_list = []
        min_speed = 30
        FORWARD = True

        while True:

            ##### perception ######
            # ultrasonic sensing
            dist_mov_ave = car.DistMeasureMovingAverage()

            motor_speed = -pid.UpdateOutput(dist_mov_ave)

            if motor_speed >= 0:
def main():

    actor_list = []
    verboseIsEnabled = None
    try:
        """
        Section for starting the client and connecting to the server
        """
        client = carla.Client('localhost', 2000)
        client.set_timeout(2.0)

        for arg in sys.argv:
            if (arg == '--verbose'):
                verboseIsEnabled = True

        if verboseIsEnabled:
            print('client version: %s' % client.get_client_version())
            print('server version: %s' % client.get_server_version())
            print('client to server connection status: {}'.format(
                client.get_server_version()))

            print('Retrieving the world data from server...')

        world = client.get_world()
        if verboseIsEnabled:
            print('{} \n'.format(world))
        """
        Section for retrieving the blueprints and spawn the actors
        """
        blueprint_library = world.get_blueprint_library()
        if verboseIsEnabled:
            print('\nRetrieving CARLA blueprint library...')
            print('\nobject: %s\n\nblueprint methods: %s\n\nblueprint list:' %
                  (type(blueprint_library), dir(blueprint_library)))
            for blueprint in blueprint_library:
                print(blueprint)

        audi_blueprint = blueprint_library.find('vehicle.audi.tt')
        print('\n%s\n' % audi_blueprint)

        color = '191,191,191'
        audi_blueprint.set_attribute('color', color)

        transform = carla.Transform(carla.Location(x=10.5, y=-1.8, z=38.5),
                                    carla.Rotation(yaw=0.0))

        vehicleEgo = world.spawn_actor(audi_blueprint, transform)
        actor_list.append(vehicleEgo)
        print('created %s' % vehicleEgo.type_id)

        color = random.choice(
            audi_blueprint.get_attribute('color').recommended_values)
        audi_blueprint.set_attribute('color', color)
        """
        Section for initializing the PID testing
        """
        user_input_sp = None
        while (not isinstance(user_input_sp, int)) and (not isinstance(
                user_input_sp, float)):
            user_input_sp = input('Enter the desired Setpoint:\n')
        data = TestData(DataInit.total_duration, DataInit.sampling_period)
        start = time.time()

        print(
            '\nStarting test:\n\n' +
            'Time(s) current_vel(m/s) setpoint_vel(m/s) throttle(%) pid_demand'
        )
        time.sleep(2.5)
        print(
            '.................................................................\n'
        )
        time.sleep(1)

        # raise SystemExit

        p = PID(DataInit.K['Kp'], DataInit.K['Ki'], DataInit.K['Kd'])
        p.setPoint(user_input_sp)
        p.Integrator_min = -5
        p.Integrator_max = 40
        pid = 0
        for _ in range(
                int(DataInit.total_duration / DataInit.sampling_period) + 1):
            measurement_value = vehicleEgo.get_velocity().x
            vehicleEgo.apply_control(carla.VehicleControl(
                pid)) if 1 > pid > 0 else vehicleEgo.apply_control(
                    carla.VehicleControl(1))
            if 0 > pid:
                vehicleEgo.apply_control(carla.VehicleControl(brake=abs(pid)))
            pid = p.update(measurement_value)
            data.append_data(round(time.time() - start, 2), p.getSetPoint(),
                             round(vehicleEgo.get_velocity().x, 5),
                             p.getError())
            time.sleep(DataInit.sampling_period)

            print('%0.3f\t%0.2f\t\t\t%0.2f\t\t%0.2f\t%0.2f' %
                  (time.time() - start, vehicleEgo.get_velocity().x,
                   p.set_point, vehicleEgo.get_control().throttle, pid))

        data.plot()
        print('\nError Mean (Steady State):\n' + str(
            round(
                np.absolute(
                    np.mean(data.error[data.error.shape[0] /
                                       2:data.error.shape[0]])), 5) * 100) +
              '%\n')

    finally:
        print('destroying actors')
        for actor in actor_list:
            actor.destroy()
        print('done.')
Example #5
0
class Blender():

  def __init__(self):
    body_id = sml_setup.Get_Parameter(NODE_NAME,'body_id',8)
    body_array = sml_setup.Get_Parameter(NODE_NAME,'body_array',[1,2])
    self.PID = PID()
    self.avoidance = AvoidanceController(body_id,body_array)
    rospy.init_node(NODE_NAME)
    self.N_yaw = sml_setup.Get_Parameter(NODE_NAME,"PID_N_yaw",500)
    self.K_yaw = sml_setup.Get_Parameter(NODE_NAME,"PID_K_yaw",2)
    self.w_inf = sml_setup.Get_Parameter(NODE_NAME,"PID_w_inf",5)
    self.Ktt = sml_setup.Get_Parameter(NODE_NAME,"PID_Ktt",1000)/(20*math.pi/180)
    self.Kphi = sml_setup.Get_Parameter(NODE_NAME,"PID_Kphi",1000)/(20*math.pi/180)
    self.CONTROL_MIN = sml_setup.Get_Parameter(NODE_NAME,"PID_CONTROL_MIN",1000)
    self.CONTROL_NEUTRAL = sml_setup.Get_Parameter(NODE_NAME,"PID_CONTROL_NEUTRAL",1500)
    self.CONTROL_MAX = sml_setup.Get_Parameter(NODE_NAME,"PID_CONTROL_MAX",2000)
    self.CONTROL_ARMING_MIN = sml_setup.Get_Parameter(NODE_NAME,"PID_CONTROL_ARMING_MIN",1025)
    self.CONTROL_CANCEL_GRAVITY = sml_setup.Get_Parameter(NODE_NAME,"PID_CONTROL_CANCEL_GRAVITY",1400)
    self.obstacle_avoidance = sml_setup.Get_Parameter(NODE_NAME,"obstacle_avoidance","False")
    
  def init_subscriptions(self, target_point,current_point, instr):
    #Subcribe to /trajectroy_gen/target to get target position, velocity and acceleration
    rospy.Subscriber('trajectory_gen/target',QuadPositionDerived,self.New_Point,target_point)
    #Subscribe to /derivator/pos_data to get position, velocity and acceleration
    rospy.Subscriber('security_guard/data_forward',QuadPositionDerived,self.New_Point,current_point)
    #Subscribe to /security_guard/controller to get permission to publish to rc/override
    rospy.Subscriber('security_guard/controller',Permission,self.Get_Permission,instr)

  def Get_Permission(self,data,instruction_obj):
    if instruction_obj.permission:
      if not data.permission:
        instruction_obj.permission=False

    if not instruction_obj.start:
      if data.permission:
        instruction_obj.start=True

  def Wait_For_Security_Guard(self,obj):
    rate=rospy.Rate(30)
    rospy.loginfo('['+NODE_NAME+']: Waiting for security guard ...')
    while not obj.start:
      if rospy.is_shutdown():
        return 
      rate.sleep()

  def Run_Blender(self):
    loop_rate=rospy.Rate(30)
    instr=Instruction()
    current_point=Point()
    target_point=Point()
    my_id = sml_setup.Get_Parameter(NODE_NAME,'body_id',1)
    bodies = sml_setup.Get_Parameter(NODE_NAME,'body_array',[1,2])
    #Publish to RC Override
    rc_override=rospy.Publisher('mavros/rc/override',OverrideRCIn,queue_size=10)

    self.init_subscriptions(target_point, current_point, instr)

    data_init=OverrideRCIn()
    command=[0,0,self.CONTROL_ARMING_MIN,0,0,0,0,0]
    data_init.channels=command

    #Wait until the security guard is online
    self.Wait_For_Security_Guard(instr)

    #integral term initialized to 0
    self.PID.set_d_updated(0)
    while not rospy.is_shutdown():
                

      if not target_point.first_point_received:
	self.Wait_For_First_Point(target_point,rc_override,data_init,loop_rate)
	#reinitialize d_updated
	self.PID.set_d_updated(0)
      d = self.PID.get_d_updated()
      x,x_vel,x_acc=self.PID.Get_Pos_Vel_Acc(current_point)  
      x_target,x_vel_target,x_acc_target=self.PID.Get_Pos_Vel_Acc(target_point) 
      u = self.blend(current_point, target_point,d)
      command_controlled = self.get_controloutput(u,x,x_target)
  
      #If OK from security guard, publish the messages via Mavros to the drone
      if instr.permission:
        data=OverrideRCIn()
        data.channels=command_controlled
        rc_override.publish(data)
  	
		
		

      loop_rate.sleep()
                
    
   

  def get_controloutput(self,u,x,x_target):
    AUX=[]
    AUX_rot=[]
    AUX.append(u[0])
    AUX.append(u[1])
    AUX.append(9.8+u[2])

    #take into consideration the yaw angle
    AUX_rot.append(math.cos(math.radians(-x[3]))*AUX[0]-math.sin(math.radians(-x[3]))*AUX[1])
    AUX_rot.append(math.sin(math.radians(-x[3]))*AUX[0]+math.cos(math.radians(-x[3]))*AUX[1])
    AUX_rot.append(AUX[2])

    norm_AUX=math.sqrt(math.pow(AUX_rot[0],2)+math.pow(AUX_rot[1],2)+math.pow(AUX_rot[2],2))

    #yaw control:
    diff=self.AngularDifference(x[3],x_target[3])
    w_yaw=-self.K_yaw*(math.radians(diff))

    #set values:
    throttle=(self.CONTROL_CANCEL_GRAVITY/9.8)*norm_AUX
    yaw=self.CONTROL_NEUTRAL - self.N_yaw*self.Saturation(w_yaw/self.w_inf,-1,1)
    pitch=self.CONTROL_NEUTRAL-self.Ktt*math.asin(AUX_rot[0]/norm_AUX)
    roll=self.CONTROL_NEUTRAL-self.Kphi*math.asin(AUX_rot[1]/norm_AUX)

    #if pitch<1400 or pitch>1600:
       #print(pitch)

    #if roll<1400 or roll>1600:
      #print(roll)

  #Implement some saturation
    throttle=self.Saturation(throttle,1000,2000)
    pitch=self.Saturation(pitch,1350,1650)
    roll=self.Saturation(roll,1350,1650)

    return [roll,pitch,throttle,yaw,0,0,0,0]


  def blend(self,current_point,target_point,d_updated):
    u = [0.,0.,0.]
    u_pid = self.PID.get_PID_output(current_point,target_point)
    u_obst = self.avoidance.get_potential_output()
    if self.obstacle_avoidance:
      alpha = self.avoidance.get_blending_constant()
    else:
      alpha = 0
    for i in range(0,2):
      u[i] = alpha * u_obst[i] + (1-alpha) * u_pid[i]
    u[2] = u_pid[2] 
    return u
 

  def New_Point(self,data,point_obj):
    if not point_obj.first_point_received:
      point_obj.first_point_received=True

    point_obj.update_point(data)

  def Saturation(self,value,minimum,maximum):

    value=max(minimum,min(maximum,value))

    return value


  def Wait_For_First_Point(self,target_obj,channel,data,rate):
    rospy.loginfo('['+NODE_NAME+']: Waiting for first point ...')
    while not target_obj.first_point_received:
      #publish low value on the throttle channel, so the drone does not disarm while waiting
      channel.publish(data)
      rate.sleep()

    rospy.loginfo('['+NODE_NAME+']: First point received')      

  def AngularDifference(self,current_angle,target_angle):
    ang_diff=current_angle-target_angle

    if math.fabs(ang_diff)>180:
      if ang_diff>0:
        ang_diff=ang_diff-360
      else:
        ang_diff=ang_diff+360

    return ang_diff
def main():

    actor_list = []
    verboseIsEnabled = None
    try:
        """
        Section for starting the client and connecting to the server
        """
        client = carla.Client('localhost', 2000)
        client.set_timeout(2.0)

        for arg in sys.argv:
            if (arg == '--verbose'):
                verboseIsEnabled = True

        if verboseIsEnabled:
            print('client version: %s' % client.get_client_version())
            print('server version: %s' % client.get_server_version())
            print('client to server connection status: {}'.format(client.get_server_version()))

            print('Retrieving the world data from server...')

        world = client.get_world()
        if verboseIsEnabled:
            print('{} \n'.format(world))

        """
        Section for retrieving the blueprints and spawn the actors
        """
        blueprint_library = world.get_blueprint_library()
        if verboseIsEnabled:
            print('\nRetrieving CARLA blueprint library...')
            print('\nobject: %s\n\nblueprint methods: %s\n\nblueprint list:' % (type(blueprint_library), dir(blueprint_library)) )
            for blueprint in blueprint_library:
                print(blueprint)

        audi_blueprint = blueprint_library.find('vehicle.audi.tt')
        print('\n%s\n' % audi_blueprint)

        color = '191,191,191'
        audi_blueprint.set_attribute('color', color)

        transform = carla.Transform(
			carla.Location(
                x=10.5, y=-1.8,
            z=38.5),carla.Rotation(yaw=0.0)
		)

        vehicleEgo = world.spawn_actor(audi_blueprint, transform)
        actor_list.append(vehicleEgo)
        print('created %s' % vehicleEgo.type_id)

        color = random.choice(audi_blueprint.get_attribute('color').recommended_values)
        audi_blueprint.set_attribute('color', color)


        """
        Section for initializing the PID testing
        """
        spd_profile = SpeedProfile()
        spd_data = spd_profile.input_file()

        print('\nStarting test:\n\n' + 'Time(s) current_vel(m/s) setpoint_vel(m/s) throttle(%) pid_demand')
        time.sleep(1.25)
        print('.................................................................\n')
        time.sleep(.5)

        input_sp = 0
        data = TestData(DataInit.total_duration, DataInit.sampling_period, spd_data)
        pid = 0
        cycle_counter = 1
        init_index = 0
        moving_list = []

        p = PID(
                DataInit.K['Kp'], 
                DataInit.K['Ki'],
                DataInit.K['Kd']
                )
        p.setPoint(input_sp)
        p.setWindup(*DataInit.windup_values)
        start = time.time()
        for _ in range(int(DataInit.total_duration / DataInit.sampling_period) + 1):
            measurement_value = vehicleEgo.get_velocity().x
            current_time = time.time()
            init_index = apply_dynamic_sp(
                p,
                cycle_counter,
                spd_data,
                current_time,
                start,
                init_index,
                moving_list,
                DataInit.response_time_offset
                )
            apply_control(pid, vehicleEgo)
            pid = p.update(measurement_value)
            data.append_data(round(current_time - start, 2), p.getSetPoint(), round(vehicleEgo.get_velocity().x, 5), p.getError())

            print('%0.3f\t%0.2f\t\t\t%0.2f\t\t%0.2f\t%0.2f' % (time.time() - start,
                                                                vehicleEgo.get_velocity().x,
                                                                p.set_point,
                                                                vehicleEgo.get_control().throttle,
                                                                pid))
            if DataInit.force_sampling_period:
                time.sleep(DataInit.sampling_period)
            cycle_counter += 1

        data.plot()
        # print('\nError Mean (if Steady State):\n' + 
        #     str(round(np.absolute(np.mean(data.error[data.error.shape[0]/2:data.error.shape[0]])), 5)) + 
        #     '%\n')

    finally:
        print('destroying actors')
        for actor in actor_list:
            actor.destroy()
        print('done.')