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)
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")
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.')
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.')
