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