diff --git a/src/navigation_node.py b/src/navigation_node.py index 04c3197..1b70349 100755 --- a/src/navigation_node.py +++ b/src/navigation_node.py @@ -1,87 +1,60 @@ #!/usr/bin/python # coding=utf-8 -# Import the ROS libraries, and load the manifest file which through -## will give us access to the project dependencies import rospy import tf2_ros as tf from tf2_geometry_msgs import PoseStamped, PointStamped import numpy as np # Import the messages we're interested in sending and receiving -from ardrone_autonomy.msg import Navdata # for receiving navdata feedback +from ardrone_autonomy.msg import Navdata # for receiving navdata feedback from ardrone_carrier.msg import NavigationGoal -from geometry_msgs.msg import Twist # for sending commands to the drone +from geometry_msgs.msg import Twist # for sending commands to the drone from tf.transformations import euler_from_quaternion from pid import PID - -import roslib; roslib.load_manifest('ardrone_tutorials') - - -# Other imports -# ??? - -# Some Constants -TARGET_TOPIC_NAME = '/pose_goal' -TARGET_TOPIC_TYPE = NavigationGoal - -# EST_POSE_TOPIC_NAME = '/ardrone/navdata' #TODO: check the name -# EST_POSE_TOPIC_TYPE = Navdata - TF_ARDRONE = 'ardrone_base_link' -TF_TARGET = 'pose_goal' # Not a real tf but should be defined by target msg +TF_TARGET = 'pose_goal' # Not a real tf but should be defined by target msg TF_FIXED = 'odom' LOOP_RATE = 10. #  [Hz] rate of the ROS node loop TARGET_THRESHOLD_POSE = 5e-2 -TARGET_THRESHOLD_ORIENTATION = np.pi/18. +TARGET_THRESHOLD_ORIENTATION = np.pi / 18. TARGET_THRESHOLD_SPEED = 0.1 def normalize_angle(rad): - return ((rad+np.pi)%(2*np.pi)) - np.pi + return ((rad + np.pi) % (2 * np.pi)) - np.pi class ArdroneNav: - """ Class for Ardrone navigation: getting target position (and estimated one) and - return velocity command to the drone with PID control""" - + """ + Class for Ardrone navigation: getting target position (and estimated one) and + return velocity command to the drone with PID control + """ def __init__(self): - """Init function (to be completed)""" - - ## Tf attributes + """ + Init function (to be completed) + """ + # Tf attributes self.tf_buffer = tf.Buffer() self.tf_listener = tf.TransformListener(self.tf_buffer) self.tf_ardrone = TF_ARDRONE self.tf_target = TF_TARGET self.tf_fixed = TF_FIXED - - #################### - ## Ardrone topics ## - #################### - - # Allow the controller to publish to the /cmd_vel topic and thus control the drone - self.pub_command = rospy.Publisher('/cmd_vel', Twist, queue_size=1) - - # Setup regular publishing of control packets - self.command = Twist() - - # rate = rospy.Rate(Hertz) à mettre dans le noeud + # ROS publishers/subscribers + self.pub_command = rospy.Publisher('/cmd_vel', Twist, queue_size=1) # drone speed command + self.sub_target_pos = rospy.Subscriber('/pose_goal', NavigationGoal, self.pose_goal_callback) ####################################### ## Positions (target and estimated) ### ####################################### - self.sub_target_pos = rospy.Subscriber(TARGET_TOPIC_NAME, TARGET_TOPIC_TYPE, - self.read_target_pose) - - # Units will be meters, seconds, and radiants. - - ## velocities (linear and angular) of the drone at current time + # Units will be meters, seconds, and radians. + # velocities (linear and angular) of the drone at current time self.target_pose = PoseStamped() self.command_pose = {'position': [0.0, 0.0, 0.0], 'orientation': [0.0, 0.0, 0.0]} # Target pose with Euler angle @@ -90,103 +63,94 @@ def __init__(self): self.vel_constrain = {'position': [1., 1., 1.], 'orientation': [0.7, 0.7, 0.7]} - self.mode = 0 # RELATIVE or ABSOLUTE for target - + self.mode = NavigationGoal.ABSOLUTE # RELATIVE or ABSOLUTE for target self.bool_command = False - - self.has_started = False # node hasnt received any command yet (to avoid computing PID on null pos) - - self.has_reached_target = False # When reaching target with a specific threshold, stop the PID + self.has_started = False # node hasn't received any command yet (to avoid computing PID on null pos) + self.has_reached_target = False # When reaching target with a specific threshold, stop the PID self.i_loop = 0 + ########## ## PID ### ########## + # Setup regular publishing of control packets + self.command = Twist() + # if no angle target is given, only compute command for position self.no_quaternion = False - # Differents weights for each composant + # Different weights for each composant self.p = {'position': [0.5, 0.5, 2.], 'orientation': [0.0, 0.0, 0.0]} self.i = {'position': [0.1, 0.05, 0.3], 'orientation': [0.0, 0.0, 0.0]} self.d = {'position': [0.1, 0.1, 0.2], 'orientation': [0.0, 0.0, 0.0]} - self.dt = {'position': [1./LOOP_RATE, 1./LOOP_RATE, 1./LOOP_RATE], - 'orientation': [1./LOOP_RATE, 1./LOOP_RATE, 1./LOOP_RATE]} - - self.pids = {} - self.pids['position'] = [PID(kp, ki, kd, dt) for (kp, ki, - kd, dt) in zip(self.p['position'], - self.i['position'], - self.d['position'], - self.dt['position'])] - - self.pids['orientation'] = [PID(kp, ki, kd, dt) for (kp, ki, - kd, dt) in zip(self.p['orientation'], - self.i['orientation'], - self.d['orientation'], - self.dt['orientation'])] + self.dt = {'position': [1. / LOOP_RATE, 1. / LOOP_RATE, 1. / LOOP_RATE], + 'orientation': [1. / LOOP_RATE, 1. / LOOP_RATE, 1. / LOOP_RATE]} + + self.pids = {'position': [PID(kp, ki, kd, dt) + for (kp, ki, kd, dt) in zip(self.p['position'], + self.i['position'], + self.d['position'], + self.dt['position'])], + 'orientation': [PID(kp, ki, kd, dt) + for (kp, ki, kd, dt) in zip(self.p['orientation'], + self.i['orientation'], + self.d['orientation'], + self.dt['orientation'])]} # If constraints on velocity are defined, activate saturation in PID - for id in range(len(self.pids['position'])): - vel_cstr = self.vel_constrain['position'][id] + for idx in range(len(self.pids['position'])): + vel_cstr = self.vel_constrain['position'][idx] if vel_cstr != 0.0: - self.pids['position'][id].activate_command_saturation(-vel_cstr, vel_cstr) - print(self.pids['position'][id].saturation_activation) + self.pids['position'][idx].activate_command_saturation(-vel_cstr, vel_cstr) + print(self.pids['position'][idx].saturation_activation) - for id in range(len(self.pids['orientation'])): - vel_cstr = self.vel_constrain['orientation'][id] + for idx in range(len(self.pids['orientation'])): + vel_cstr = self.vel_constrain['orientation'][idx] if vel_cstr != 0.0: - self.pids['orientation'][id].activate_command_saturation(-vel_cstr, vel_cstr) - print(self.pids['orientation'][id].saturation_activation) - - - + self.pids['orientation'][idx].activate_command_saturation(-vel_cstr, vel_cstr) + print(self.pids['orientation'][idx].saturation_activation) def set_command(self, command): - """Define the command msg (Twist) to be published to the drone""" + """ + Define the command msg (Twist) to be published to the drone + """ # WARNING: cmd_vel is not the wanted velocity we want the drone to have: only command +-1 ... - # WARNING: angular.x, angular.y should be zero, except in hover mode + # WARNING: angular.x, angular.y should be zero, in order to stay in hover mode vx = command['position'][0] vy = command['position'][1] vz = command['position'][2] - - rotX = command['orientation'][0] - rotY = command['orientation'][1] rotZ = command['orientation'][2] - self.command.linear.x = vx # should be in mm/s + self.command.linear.x = vx # TODO : check if it should be in mm/s self.command.linear.y = vy self.command.linear.z = vz self.command.angular.x = 0.0 self.command.angular.y = 0.0 - self.command.angular.z = rotZ # should be in degree/s - + self.command.angular.z = rotZ # TODO : check if it should be in degree/s def set_hover(self): - """ set command to 0 for hovering""" - - self.command.linear.x = 0.0 # should be in mm/s - self.command.linear.y = 0.0 - self.command.linear.z = 0.0 - self.command.angular.x = 0.0 - self.command.angular.y = 0.0 - self.command.angular.z = 0.0 # should be in degree/s - + """ + set command to 0 for hovering + """ + self.command = Twist() def send_command(self): - """publish the command twist msg to cmd_vel/""" - + """ + publish the command twist msg to cmd_vel + """ self.pub_command.publish(self.command) if self.bool_command: print('Command:', self.command) self.bool_command = False - - def read_target_pose(self, msg_pose): - """Get target position in specific frame. We assume that target is already expressed in the - control frame """ - + def pose_goal_callback(self, msg_pose): + """ + Get target position in specific frame. + We assume that target is already expressed in the control frame + :param msg_pose: NavigationGoal msg + """ # First time a command has been received self.has_started = True self.has_reached_target = False @@ -206,7 +170,6 @@ def read_target_pose(self, msg_pose): self.target_pose.pose.orientation.y, self.target_pose.pose.orientation.z, self.target_pose.pose.orientation.w] == [0.0, 0.0, 0.0, 0.0]): - self.no_quaternion = True # According to mode, define target pose in referential @@ -237,7 +200,6 @@ def read_target_pose(self, msg_pose): self.target_pose = self.tf_buffer.transform(self.target_pose, self.tf_fixed) - def compute_error(self): """With target expressed in target frame, compute real time error with ardrone position, with tf""" # Express target pos in drone frame, which gives the error @@ -254,7 +216,6 @@ def compute_error(self): else: rotation = [0.0, 0.0, 0.0] - # Get orientation and position into a structure for PID control self.command_pose['orientation'] = rotation self.command_pose['position'] = [command_pose.pose.position.x, @@ -263,14 +224,15 @@ def compute_error(self): # DEBUG self.i_loop += 1 - if self.i_loop%50 == 0: + if self.i_loop % 50 == 0: print(self.no_quaternion) print('Error', self.command_pose) def update(self): - """compute velocity command through PID""" - + """ + compute velocity command through PID + """ # Set empty command command = {'position': [0.0, 0.0, 0.0], 'orientation': [0.0, 0.0, 0.0]} @@ -281,22 +243,23 @@ def update(self): # Compute position command for p_id in range(len(self.pids['position'])): - command['position'][p_id] = self.pids['position'][p_id].compute_command(\ - self.command_pose['position'][p_id]) + command['position'][p_id] = self.pids['position'][p_id].compute_command( \ + self.command_pose['position'][p_id]) - #TODO: remove angle + # TODO: remove angle # if command for angle is not null, compute angle command if not self.no_quaternion: for p_id in range(len(self.pids['orientation'])): - command['orientation'][p_id] = self.pids['orientation'][p_id].compute_command(\ - self.command_pose['orientation'][p_id]) + command['orientation'][p_id] = self.pids['orientation'][p_id].compute_command( \ + self.command_pose['orientation'][p_id]) # set and send command to the drone self.set_command(command) - if np.sqrt(command['position'][0]**2 + command['position'][1]**2 + command['position'][2]**2) < TARGET_THRESHOLD_POSE and\ - np.abs(command['orientation'][2]) < TARGET_THRESHOLD_ORIENTATION: + if np.sqrt(command['position'][0] ** 2 + command['position'][1] ** 2 + command['position'][ + 2] ** 2) < TARGET_THRESHOLD_POSE and \ + np.abs(command['orientation'][2]) < TARGET_THRESHOLD_ORIENTATION: self.has_reached_target = True # if no pose goal received, do nothing, send null command