class ROSControllerNode(object): """ROS interface for controlling ARDrone, using position controller""" def __init__(self): """Initialize the ROSControllerNode class.""" self.model_name = 'ARDroneCarre' # Publishers self.pub_cmd_vel = rospy.Publisher('cmd_vel_RHC', Twist, queue_size=300) # Subscribers self.sub_vicon_data = rospy.Subscriber( '/vicon/{0}/{0}'.format(self.model_name), TransformStamped, self.process_vicon_data) self.sub_des_pos = rospy.Subscriber('des_pos', String, self.process_desired_position) # Initialize messages for publishing self.cmd_vel = Twist() # Run the onboard controller at 200 Hz self.onboard_loop_frequency = 200. # Calling the position controller to pass the data self.pos_class = PositionController() # Run this ROS node at the onboard loop frequency self.run_pub_cmd_vel = rospy.Timer( rospy.Duration(1. / self.onboard_loop_frequency), self.process_commands) # Initialize time self.nonunix_time = 0 self.old_time = rospy.get_time() self.start_time = rospy.get_time() self.dt = 0 # Initialize points self.current_point = 1 self.num_points_old = 0 def process_commands(self, event): """Determine the motor speeds and and publishes these.""" # Calculate time step current_time = rospy.get_time() self.dt = current_time - self.old_time # Get commands from the position controller commands = self.pos_class.update_position_controller(self.dt) [roll_command, pitch_command, yaw_rate_command, climb_rate_command] = commands # Set cmd_vel message values to commands self.cmd_vel.linear.x = roll_command self.cmd_vel.linear.y = pitch_command self.cmd_vel.linear.z = climb_rate_command self.cmd_vel.angular.z = yaw_rate_command # Publish commands self.pub_cmd_vel.publish(self.cmd_vel) # Update timestamp self.old_time = current_time def process_vicon_data(self, vicon_data_msg): self.pos_class.current_trans_x = vicon_data_msg.transform.translation.x self.pos_class.current_trans_y = vicon_data_msg.transform.translation.y self.pos_class.current_trans_z = vicon_data_msg.transform.translation.z self.pos_class.current_rot_x = vicon_data_msg.transform.rotation.x self.pos_class.current_rot_y = vicon_data_msg.transform.rotation.y self.pos_class.current_rot_z = vicon_data_msg.transform.rotation.z self.pos_class.current_rot_w = vicon_data_msg.transform.rotation.w def process_desired_position(self, pos_msg): # Get message containing desired trajectory coordinates from desired_positions self.des_pos_msg = np.fromstring(pos_msg.data, dtype=float, sep=' ') # Calculate total number of waypoints num_points = self.des_pos_msg.size / 4 # Update number of waypoints if num_points != self.num_points_old: self.num_points_old = num_points self.current_point = 0 # Convert 1x12 array into 3 arrays of 4 elements (x,y,z) trajectory = np.reshape(self.des_pos_msg, (-1, 4)) self.pos_class.desired_x = trajectory[self.current_point - 1, 0] # x coordinates self.pos_class.desired_y = trajectory[self.current_point - 1, 1] # y coordinates self.pos_class.desired_z = trajectory[self.current_point - 1, 2] # z coordinates self.nonunix_time += self.dt # setting deviation tolerance, the higher tolerance, the faster the drone navigates if (( (self.pos_class.desired_x - 0.5) < self.pos_class.current_trans_x < (self.pos_class.desired_x + 0.5)) and ((self.pos_class.desired_y - 0.5) < self.pos_class.current_trans_y < (self.pos_class.desired_y + 0.5)) and ((self.pos_class.desired_z - 0.5) < self.pos_class.current_trans_z < (self.pos_class.desired_z + 0.5))): if self.current_point < num_points and self.nonunix_time >= 0.01: self.current_point += 1 self.nonunix_time = 0
class ROSControllerNode(object): """ROS interface for controlling the Parrot ARDrone in the Vicon Lab.""" # write code here to define node publishers and subscribers # publish to /cmd_vel topic # subscribe to /vicon/ARDroneCarre/ARDroneCarre for position and attitude feedback def __init__(self): """Initialize the ROSControllerNode class.""" # Publishers """ self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size = 300) """ self.pub_cmd_vel = rospy.Publisher('cmd_vel_RHC', Twist, queue_size=300) self.pubTakePic = rospy.Publisher('/ardrone/take_pic', String, queue_size=10) # Subscribers self.model_name = 'ARDroneCarre' self.sub_vicon_data = rospy.Subscriber( '/vicon/{0}/{0}'.format(self.model_name), TransformStamped, self.update_vicon_data) self.sub_des_pos = rospy.Subscriber('des_pos', String, self.update_desired_position) # Initialize messages for publishing self.cmd_vel_msg = Twist() # Run the onboard controller at 200 Hz self.onboard_loop_frequency = 200. # Calling the position controller to pass the data self.pos_class = PositionController() # Run this ROS node at the onboard loop frequency self.run_pub_cmd_vel = rospy.Timer( rospy.Duration(1. / self.onboard_loop_frequency), self.update_roll_pitch_yaw) self.current_point = 1 self.nonunix_time = 0 self.dt = 0 self.num_points_old = 3 self.pic_number = 1 # Keep time for differentiation and integration within the controller self.old_time = rospy.get_time() self.start_time = rospy.get_time() def update_roll_pitch_yaw(self, event): """Determine the motor speeds and and publishes these.""" # Determine the time step for differentiation and integration current_time = rospy.get_time() self.dt = current_time - self.old_time # Get the next set of postioning commands from the position controller new_controls = self.pos_class.update_position_controller(self.dt) # Rearranging the data for ease [new_roll, new_pitch, new_yaw_rate, new_climb_rate] = new_controls # Setting the cmd_vel msg values to the desired ones self.cmd_vel_msg.linear.x = new_roll self.cmd_vel_msg.linear.y = new_pitch self.cmd_vel_msg.angular.z = new_yaw_rate self.cmd_vel_msg.linear.z = new_climb_rate # Publish the motor commands for the ardrone plugin self.pub_cmd_vel.publish(self.cmd_vel_msg) # Set the old time to the current for future time step calculations self.old_time = current_time def update_vicon_data(self, vicon_data_msg): (self.pos_class.current_trans_x, self.pos_class.current_trans_y, self.pos_class.current_trans_z) = ( vicon_data_msg.transform.translation.x, vicon_data_msg.transform.translation.y, vicon_data_msg.transform.translation.z) (self.pos_class.current_rot_x, self.pos_class.current_rot_y, self.pos_class.current_rot_z, self.pos_class.current_rot_w) = (vicon_data_msg.transform.rotation.x, vicon_data_msg.transform.rotation.y, vicon_data_msg.transform.rotation.z, vicon_data_msg.transform.rotation.w) def update_desired_position(self, pos_msg): self.des_pos_msg = np.fromstring(pos_msg.data, dtype=float, sep=' ') num_points = self.des_pos_msg.size / 4 if num_points != self.num_points_old: self.num_points_old = num_points self.current_point = 1 trajectory = np.reshape(self.des_pos_msg, (-1, 4)) self.pos_class.desired_x = trajectory[self.current_point - 1, 0] self.pos_class.desired_y = trajectory[self.current_point - 1, 1] self.pos_class.desired_z = trajectory[self.current_point - 1, 2] self.pos_class.desired_yaw = trajectory[self.current_point - 1, 3] self.nonunix_time += self.dt #--------------------------------------------------- # SCANNING PHASE. #--------------------------------------------------- self.pubTakePic.publish("no") #--------------------------------------------------- if (((self.pos_class.desired_x - 0.15) < self.pos_class.current_trans_x < (self.pos_class.desired_x + 0.15)) and ((self.pos_class.desired_y - 0.15) < self.pos_class.current_trans_y < (self.pos_class.desired_y + 0.15)) and ((self.pos_class.desired_z - 0.05) < self.pos_class.current_trans_z < (self.pos_class.desired_z + 0.05)) and ((self.pos_class.desired_yaw - 0.25) < self.pos_class.current_yaw < (self.pos_class.desired_yaw + 0.25))): if self.current_point < num_points and self.nonunix_time >= 0.01: self.current_point += 1 self.nonunix_time = 0 #--------------------------------------------------- # SCANNING PHASE. #--------------------------------------------------- current_mission_time = int(self.start_time - (rospy.get_time())) file_name = str(round(self.pic_number)) + "_t" + str( current_mission_time) + "_X" + str( round(self.pos_class.current_trans_x, 2)) + "_Y" + str( round(self.pos_class.current_trans_y, 2) ) + "_Z" + str(round( self.pos_class.current_trans_z, 2)) + "_W" + str( round(self.pos_class.current_yaw, 2)) + ".jpg" self.pic_number = self.pic_number + 1 self.pubTakePic.publish(file_name)