Untitled
unknown
plain_text
2 years ago
8.1 kB
3
Indexable
Never
#!/usr/bin/env python3
'''
This python file runs a ROS-node of name drone_control which holds the position of e-Drone on the given dummy.
This node publishes and subsribes the following topics:
PUBLICATIONS SUBSCRIPTIONS
/drone_command /whycon/poses
/alt_error /pid_tuning_altitude
/pitch_error /pid_tuning_pitch
/roll_error /pid_tuning_roll
Rather than using different variables, use list. eg : self.setpoint = [1,2,3], where index corresponds to x,y,z ...rather than defining self.x_setpoint = 1, self.y_setpoint = 2
CODE MODULARITY AND TECHNIQUES MENTIONED LIKE THIS WILL HELP YOU GAINING MORE MARKS WHILE CODE EVALUATION.
'''
# Importing the required libraries
from distutils.log import error
from edrone_client.msg import *
from geometry_msgs.msg import PoseArray
from std_msgs.msg import Int16
from std_msgs.msg import Int64
from std_msgs.msg import Float64
from pid_tune.msg import PidTune
import rospy
import time
class Edrone():
"""docstring for Edrone"""
def __init__(self):
rospy.init_node('drone_control') # initializing ros node with name drone_control
# This corresponds to your current position of drone. This value must be updated each time in your whycon callback
# [x,y,z]
self.drone_position = [0.0,0.0,0.0]
# [x_setpoint, y_setpoint, z_setpoint]
self.setpoint = [2,2,20] # whycon marker at the position of the dummy given in the scene. Make the whycon marker associated with position_to_hold dummy renderable and make changes accordingly
#Declaring a cmd of message type edrone_msgs and initializing values
self.cmd = edrone_msgs()
self.cmd.rcRoll = 1500
self.cmd.rcPitch = 1500
self.cmd.rcYaw = 1500
self.cmd.rcThrottle = 1500
self.cmd.rcAUX1 = 1500
self.cmd.rcAUX2 = 1500
self.cmd.rcAUX3 = 1500
self.cmd.rcAUX4 = 1500
#initial setting of Kp, Kd and ki for [roll, pitch, throttle]. eg: self.Kp[2] corresponds to Kp value in throttle axis
#after tuning and computing corresponding PID parameters, change the parameters
self.Kp = [0,0,0]
self.Ki = [0,0,0]
self.Kd = [0,0,0]
#-----------------------Add other required variables for pid here ----------------------------------------------
self.prev_errors = [-2,-2,-20]
self.min_values = [1000, 1000, 1000]
self.max_values = [2000, 2000, 2000]
self.sum_of_errors = [0, 0, 0]
self.change_in_error = []
# Hint : Add variables for storing previous errors in each axis, like self.prev_values = [0,0,0] where corresponds to [pitch, roll, throttle] # Add variables for limiting the values like self.max_values = [2000,2000,2000] corresponding to [roll, pitch, throttle]
# self.min_values = [1000,1000,1000] corresponding to [pitch, roll, throttle]
# You can change the upper limit and lower limit accordingly.
#----------------------------------------------------------------------------------------------------------
# # This is the sample time in which you need to run pid. Choose any time which you seem fit. Remember the stimulation step time is 50 ms
self.sample_time = 0.060 # in seconds
# Publishing /drone_command, /alt_error, /pitch_error, /roll_error
self.command_pub = rospy.Publisher('/drone_command', edrone_msgs, queue_size=1)
#------------------------Add other ROS Publishers here-----------------------------------------------------
#Get remaining topics from the pid_tune package
#self.alt_error_pub = rospy.Publisher('/alt_error', )
#-----------------------------------------------------------------------------------------------------------
# Subscribing to /whycon/poses, /pid_tuning_altitude, /pid_tuning_pitch, pid_tuning_roll
rospy.Subscriber('whycon/poses', PoseArray, self.whycon_callback)
rospy.Subscriber('/pid_tuning_altitude',PidTune,self.altitude_set_pid)
#-------------------------Add other ROS Subscribers here----------------------------------------------------
#------------------------------------------------------------------------------------------------------------
self.arm() # ARMING THE DRONE
# Disarming condition of the drone
def disarm(self):
self.cmd.rcAUX4 = 1100
self.command_pub.publish(self.cmd)
rospy.sleep(1)
# Arming condition of the drone : Best practise is to disarm and then arm the drone.
def arm(self):
self.disarm()
self.cmd.rcRoll = 1500
self.cmd.rcYaw = 1500
self.cmd.rcPitch = 1500
self.cmd.rcThrottle = 1000
self.cmd.rcAUX4 = 1500
self.command_pub.publish(self.cmd) # Publishing /drone_command
rospy.sleep(1)
# Whycon callback function
# The function gets executed each time when /whycon node publishes /whycon/poses
def whycon_callback(self,msg):
self.drone_position[0] = msg.poses[0].position.x
#--------------------Set the remaining co-ordinates of the drone from msg----------------------------------------------
self.drone_position[1] = msg.poses[0].position.y
self.drone_position[2] = msg.poses[0].position.z
#---------------------------------------------------------------------------------------------------------------
# Callback function for /pid_tuning_altitude
# This function gets executed each time when /tune_pid publishes /pid_tuning_altitude
def altitude_set_pid(self,alt):
self.Kp[2] = alt.Kp * 0.06 # This is just for an example. You can change the ratio/fraction value accordingly
self.Ki[2] = alt.Ki * 0.008
self.Kd[2] = alt.Kd * 0.3
#----------------------------Define callback function like altitide_set_pid to tune pitch, roll--------------
def pitch_set_pid(self, pitch):
self.Kp[1] = pitch.Kp*0.06
self.Ki[1] = pitch.Ki * 0.008
self.Kd[1] = pitch.Kd * 0.3
def roll_set_pid(self, roll):
self.Kp[0] = roll.Kp*0.06
self.Ki[0] = roll.Ki * 0.008
self.Kd[0] = roll.Kd * 0.3
#----------------------------------------------------------------------------------------------------------------------
def pid(self):
#-----------------------------Write the PID algorithm here--------------------------------------------------------------
# Steps:
# 1. Compute error in each axis. eg: error[0] = self.drone_position[0] - self.setpoint[0] ,where error[0] corresponds to error in x...
# 2. Compute the error (for proportional), change in error (for derivative) and sum of errors (for integral) in each axis. Refer "Understanding PID.pdf" to understand PID equation.
# 3. Calculate the pid output required for each axis. For eg: calcuate self.out_roll, self.out_pitch, etc.
# 4. Reduce or add this computed output value on the avg value ie 1500. For eg: self.cmd.rcRoll = 1500 + self.out_roll. LOOK OUT FOR SIGN (+ or -). EXPERIMENT AND FIND THE CORRECT SIGN
# 5. Don't run the pid continously. Run the pid only at the a sample time. self.sampletime defined above is for this purpose. THIS IS VERY IMPORTANT.
# 6. Limit the output value and the final command value between the maximum(2000) and minimum(1000)range before publishing. For eg : if self.cmd.rcPitch > self.max_values[1]:
# self.cmd.rcPitch = self.max_values[1]
# 7. Update previous errors.eg: self.prev_error[1] = error[1] where index 1 corresponds to that of pitch (eg)
# 8. Add error_sum
#computing errors
error[0] = self.drone_position[0] - self.setpoint[0]
error[1] = self.drone_position[1] - self.drone_position[1]
error[2] = self.drone_position[2] - self.drone_position[2]
#computing error parameters
self.change_in_error = [error[0] - self.prev_errors[0], error[1] - self.prev_errors[1], error[2] - self.prev_errors[2]]
self.sum_of_errors += error
self.prev_errors = error
#------------------------------------------------------------------------------------------------------------------------
self.command_pub.publish(self.cmd)
if __name__ == '__main__':
e_drone = Edrone()
r = rospy.Rate() #specify rate in Hz based upon your desired PID sampling time, i.e. if desired sample time is 33ms specify rate as 30Hz
while not rospy.is_shutdown():
e_drone.pid()
r.sleep()