I have created a ros node and I want to use PID control to get the ARDRONE to fly to coordinates supplied on the command line. I'm running the ARDRONE sim in Gazebo in Singularity. I have created a pid_control.py file inside a ROS package in singularity. I know how get the coordinates of the drone using the gazebo state service, and supply control as Twist messages sent to the drone. I believe the difference between the current position and the target position is the error that I should use for PID. Also, because I will be taking discrete samples, the integral of the error is just the accumulated sum, and the derivative of the error is just the difference between the current error and the previous error. Here is my code.
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Pose, Twist
from std_msgs.msg import Empty
from gazebo_msgs.srv import GetModelState
class PIDController():
def __init__(self):
# Set up ROS node and subscribers/publishers
rospy.init_node('pid_controller', anonymous=True)
self.rate = rospy.Rate(10)
self.take_off = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=10)
self.land = rospy.Publisher('/ardrone/land', Empty, queue_size=10)
self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
rospy.wait_for_service('/gazebo/get_model_state')
self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
# PID parameters
self.kp = 1.0
self.ki = 0.0
self.kd = 0.0
# initialise prev errors
self.error_previous_x = 0.0
self.error_previous_y = 0.0
self.error_previous_z = 0.0
# initialize error sum values
self.error_sum_x = 0.0
self.error_sum_y = 0.0
self.error_sum_z = 0.0
# Set up goal coordinates
self.goal_x = input('x <= ')
self.goal_y = input('y <= ')
self.goal_z = input('z <= ')
def get_current_pose(self):
resp = self.get_model_state('quadrotor', '') # 'quadrotor' is the name of the drone in the Gazebo simulation
return resp.pose.position.x, resp.pose.position.y, resp.pose.position.z
def pid_loop(self):
# take_off...
rospy.loginfo("Taking off...")
self.take_off.publish(Empty())
rospy.sleep(1)
# moving...
print('moving')
while not rospy.is_shutdown():
# Get current position of the drone
current_x, current_y, current_z = self.get_current_pose()
# Compute error terms at 'time' t
error_x = self.goal_x - current_x
error_y = self.goal_y - current_y
error_z = self.goal_z - current_z
error_total = error_x + error_y + error_z
# compute error sums
self.error_sum_x += error_x
self.error_sum_y += error_y
self.error_sum_z += error_z
# compute error diff
error_diff_x = error_x - self.error_previous_x
error_diff_y = error_y - self.error_previous_y
error_diff_z = error_z - self.error_previous_z
# update U
U_x = self.kp * error_x + self.ki * self.error_sum_x + self.kd * error_diff_x
U_y = self.kp * error_y + self.ki * self.error_sum_y + self.kd * error_diff_y
U_z = self.kp * error_z + self.ki * self.error_sum_z + self.kd * error_diff_z
U_total = U_x + U_y + U_z
# how do I update self.kp, self.ki, self.kd?
# Update error_previous
self.error_previous_x = error_x
self.error_previous_y = error_y
self.error_previous_z = error_y
# Compute control command
control_command = Twist()
control_command.linear.x = U_x
control_command.linear.y = U_y
control_command.linear.z = U_z
# move...
self.velocity_publisher.publish(control_command)
self.rate.sleep()
# rospy.sleep(1)
# print('U_x={}, U_y={}, U_z={}'.format(U_x, U_y, U_z))
print('error_total={}'.format(error_total))
print('U_total={}'.format(U_total))
if abs(error_total) <= 0.5:
# halt!
control_command.linear.x = 0.0
control_command.linear.y = 0.0
control_command.linear.z = 0.0
control_command.angular.x = 0.0
control_command.angular.y = 0.0
control_command.angular.z = 0.0
self.velocity_publisher.publish(control_command)
rospy.sleep(1)
# land
# rospy.loginfo("Landing...")
# self.land.publish(Empty())
# rospy.sleep(3)
break
if __name__ == '__main__':
try:
controller = PIDController()
controller.pid_loop()
except rospy.ROSInterruptException:
print('EXITED')
I have only setup the PID variables in a way that I believe I understand. So far they are not being updated in the control loop and I am not sure how to go about doing that. For now, the drone does fly to some location using the error values only.
