def __init__(self): """ROS interface for controlling the Parrot ARDrone in the Vicon Lab.""" self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32) #self.random_test = rospy.Publisher('/random_stuff', Twist, queue_size = 32) self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_vicon) #self.start_value = rospy.Subscriber('/start_execution', String, self.run_stuff) self.desired_position_data = rospy.Subscriber( '/desired_positions', PoseStamped, self.update_desired_position) #self.pub_check = rospy.Publisher('/check_mate', String, queue_size = 100) self.pub_reached = rospy.Publisher('/check_reach', String, queue_size=100) self.pub_counter = 0 self.translation_x_old = 0.0 self.translation_y_old = 0.0 self.translation_z_old = 0.0 self.z_velocity_old = 0.0 self.translation_x_desired = 0.0 self.translation_y_desired = 0.0 self.translation_z_desired = 0.5 self.translation_x = 0.0 self.translation_y = 0.0 self.translation_z = 0.0 self.rotation_x = 0.0 self.rotation_y = 0.0 self.rotation_z = 0.0 self.rotation_w = 1.0 self.rotation_x_desired = 0.0 self.rotation_y_desired = 0.0 self.rotation_z_desired = 0.0 self.rotation_w_desired = 1.0 self.time_interval = 0 self.current_time = rospy.get_time() self.old_time = rospy.get_time() self.postcom = PositionController() # Run the onboard controller at 100 Hz self.onboard_loop_frequency = 100. # Run this ROS node at the onboard loop frequency self.nutjobcase = rospy.Timer( rospy.Duration(1. / self.onboard_loop_frequency), self.process_commands) self.postcom = PositionController() """
def __init__(self): self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32) self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_vicon) self.desired_position_data = rospy.Subscriber( '/desired_positions', PoseStamped, self.update_desired_position) self.start_stop_command = rospy.Subscriber('/start_stop_toggle', String, self.update_state) self.pub_errors = rospy.Publisher('/errors', PoseStamped, queue_size=100) #self.pub_check = rospy.Publisher('/check_mate', String, queue_size = 100) self.run_state = False self.translation_x_old = 0.0 self.translation_y_old = 0.0 self.translation_z_old = 0.0 self.z_velocity_old = 0.0 self.translation_x_desired = 0.0 self.translation_y_desired = 0.0 self.translation_z_desired = 0.0 self.translation_x = 0.0 self.translation_y = 0.0 self.translation_z = 0.0 self.rotation_x = 0.0 self.rotation_y = 0.0 self.rotation_z = 0.0 self.rotation_w = 1.0 self.rotation_x_desired = 0.0 self.rotation_y_desired = 0.0 self.rotation_z_desired = 0.0 self.rotation_w_desired = 1.0 self.time_interval = 0 self.current_time = rospy.get_time() self.old_time = rospy.get_time() self.postcom = PositionController() # Run the publish commands at 100 Hz self.onboard_loop_frequency = 100. # Run this ROS node at the onboard loop frequency self.nutjobcase = rospy.Timer( rospy.Duration(1. / self.onboard_loop_frequency), self.run_process) self.postcom = PositionController()
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 __init__(self): # Publishers: rotor velocities self.pub_rotor_vel = rospy.Publisher('/crazyflie2/command/motor_speed', Actuators, queue_size=1) # Subscribers: desired posn, current posn #self._goal_msg = MultiDOFJointTrajectory() #self.sub_goal = rospy.Subscriber('crazyflie2/command/trajectory', MultiDOFJointTrajectory, self._goal_callback) self._currpos_msg = Odometry() self.sub_currpos = rospy.Subscriber('/crazyflie2/odometry', Odometry, self._currpos_callback) # end of publisher/subscriber # initialize other parameters self.data_list = [[ 'Time', 'Desired_x', 'Desired_y', 'Desired_z', 'Actual_x', 'Actual_y', 'Actual_z', 'Desired_yaw', 'Desired_roll', 'Desired_pitch', 'Desired_yaw_r', 'Desired_climb_r', 'Actual_roll', 'Actual_pitch', 'Actual_yaw_r', 'Actual_climb_r' ]] self.start = [1.0, 1.0] self.wp_id = 1 self.previous_time = 0.0 self.change_time = -100.0 self.controller = PositionController() self.init_time = rospy.get_time() # initialize position and roll, pitch, yaw self._x = 0 self._y = 0 self._z = 0 self.roll = 0 self.pitch = 0 self.yaw = 0 self._z_old = 0.0 self._z_oold = 0.0 self.yaw_old = 0.0
def __init__(self): # Publishers self.pub_vel_cmd = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32) self.pub_flat_state = rospy.Publisher('/aer1217/flat_state', FlatState, queue_size=32) #Subscriber self.sub_vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_quadrotor_state) self.sub_flat_in = rospy.Subscriber('/aer1217/flat_in', FlatInput, self.update_flat_in) self.sub_flat_state_op = rospy.Subscriber('/aer1217/flat_state_op', FlatState, self.update_flat_state_op) self.sub_desired_pos = rospy.Subscriber('/aer1217/desired_position', TransformStamped, self.update_desired_pos) #initialize variables self.actual_pos = TransformStamped() self.actual_pos_prev = TransformStamped() self.actual_vel = np.array([0, 0, 0, 0]) self.actual_vel_prev = np.array([0, 0, 0, 0]) self.actual_vel_prev2 = np.array([0, 0, 0, 0]) self.actual_vel_prev3 = np.array([0, 0, 0, 0]) self.flat_state_msg = FlatState() self.desired_pos = TransformStamped() self.desired_pos.transform.translation.z = 2 #initialize z to 2 self.desired_pos_prev = TransformStamped() self.desired_vel = np.array([0, 0, 2, 0]) self.flat_state = list(np.zeros(9)) self.flat_input = list(np.zeros(4)) self.flat_state_op = list(np.zeros(9)) #initialize position controller self.pos_controller = PositionController() self.flat_input = np.array([0, 0, 0, 0]) #publish vel commands self.dt = 1 / 300.0 self.freq = 300 #run ros loop rate = rospy.Rate(self.freq) while not rospy.is_shutdown(): self.send_vel_cmd() rate.sleep()
def __init__(self): # Publishers self.pub_vel_cmd = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32) self.pub_error = rospy.Publisher('/aer1217/position_error', Twist, queue_size=32) #Subscriber self.sub_vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_quadrotor_state) self.sub_desired_pos = rospy.Subscriber('/aer1217/desired_position', TransformStamped, self.update_desired_pos) #initialize variables self.actual_pos = TransformStamped() self.actual_pos_prev = TransformStamped() self.actual_vel = np.array([0, 0, 0, 0]) self.desired_pos = TransformStamped() self.desired_pos.transform.translation.z = 2 #initialize z to 2 self.desired_pos_prev = TransformStamped() self.desired_vel = np.array([0, 0, 2, 0]) #Define Published msgs self.pos_error = Twist() self.vel_cmd_msg = Twist() #initialize position controller self.pos_controller = PositionController() #publish vel commands self.dt = 1 / 100.0 #press_key_wait = rospy.wait_for_message('/aer1217/move_type', Int16) self.pub_prop_vel = rospy.Timer(rospy.Duration(self.dt), \ self.send_vel_cmd)
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 __init__(self): self.position_controller = PositionController() self.sub_desired_state = rospy.Subscriber( '/aer1217_ardrone/desired_state', DesiredStateMsg, self.update_quadrotor_desired_state) ####################################################################### # input is the estimated state x,y,z,roll,pitch,yaw from VICON self.sub_estimated_state_vicon = rospy.Subscriber( '/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_quadrotor_estimated_state_from_vicon) ####################################################################### # output roll,pitch,yaw_rate,climb_rate commands self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=30) self.pub_cmd_vel_RHC = rospy.Publisher('cmd_vel_RHC', Twist, queue_size=30) ####################################################################### self.onboard_loop_frequency = 200. self.pub_cmd_vel_timer = rospy.Timer( rospy.Duration(1. / self.onboard_loop_frequency), self.send_cmd_vel)
def process_commands(self): self.rotation = np.array([ self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w ]) self.rotation_desired = np.array([ self.rotation_x_desired, self.rotation_y_desired, self.rotation_z_desired, self.rotation_w_desired ]) current_time = rospy.get_time() self.time_interval = current_time - self.old_time self.old_time = current_time postcom = PositionController( self.translation_x_old, self.translation_y_old, self.translation_z_old, self.translation_x, self.translation_y, self.translation_z, self.rotation, self.translation_x_desired, self.translation_y_desired, self.translation_z_desired, self.rotation_desired, self.z_velocity_old, self.time_interval) returnvalue = postcom.member() """ roll = returnvalue[0] pitch = returnvalue[1] yaw = returnvalue[2] z_dot = returnvalue[3] old_x = returnvalue[4] old_y = returnvalue[5] old_z = returnvalue[6] old_velocity_z = returnvalue[7] """ [roll, pitch, yaw, z_dot, old_x, old_y, old_z, old_velocity_z] = returnvalue yaw_dot = 0 self.set_vel(roll, pitch, z_dot, yaw) self.store_old_values(old_x, old_y, old_z, old_velocity_z)
class ROSControllerNode(object): #Initialize Ros Controller def __init__(self): self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32) self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_vicon) self.desired_position_data = rospy.Subscriber( '/desired_positions', PoseStamped, self.update_desired_position) self.start_stop_command = rospy.Subscriber('/start_stop_toggle', String, self.update_state) self.request_pic = rospy.Publisher('/take_pic', String, queue_size=10) self.request_land = rospy.Publisher('/ask_land', String, queue_size=2) self.pub_errors = rospy.Publisher('/errors', PoseStamped, queue_size=100) #self.pub_check = rospy.Publisher('/check_mate', String, queue_size = 100) self.run_state = False self.translation_x_old = 0.0 self.translation_y_old = 0.0 self.translation_z_old = 0.0 self.z_velocity_old = 0.0 self.translation_x_desired = 0.0 self.translation_y_desired = 0.0 self.translation_z_desired = 0.0 self.translation_x = 0.0 self.translation_y = 0.0 self.translation_z = 0.0 self.rotation_x = 0.0 self.rotation_y = 0.0 self.rotation_z = 0.0 self.rotation_w = 1.0 self.rotation_x_desired = 0.0 self.rotation_y_desired = 0.0 self.rotation_z_desired = 0.0 self.rotation_w_desired = 1.0 self.pic_counter = 0 self.pic_x = [ 0, -2, -2, -2, -2, -2, -2, -2, -2, -2, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2 ] self.pic_y = [ 0, -2.0000, -1.5000, -1.0000, -0.5000, 0, 0.5000, 1.0000, 1.5000, 2.0000, 2.0000, 1.5000, 1.0000, 0.5000, 0, -0.5000, -1.0000, -1.5000, -2.0000, -2.0000, -1.5000, -1.0000, -0.5000, 0, 0.5000, 1.0000, 1.5000, 2.0000, 2.0000, 1.5000, 1.0000, 0.5000, 0, -0.5000, -1.0000, -1.5000, -2.0000, -2.0000, -1.5000, -1.0000, -0.5000, 0, 0.5000, 1.0000, 1.5000, 2.0000 ] self.pic_z = [ 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3 ] self.waypoint_counter = 0 x = np.loadtxt( '/home/mason/aer1217/labs/src/aer1217_ardrone_simulator/scripts/order.txt', delimiter=None) self.order_pic = x.astype(int) self.x_position_array = [1, 4.26, 0.88, 4.33, 7.69, 1] self.y_position_array = [1, 1.23, 5.48, 8.04, 4.24, 1] self.time_interval = 0 self.current_time = rospy.get_time() self.old_time = rospy.get_time() self.postcom = PositionController() # Run the publish commands at 100 Hz self.onboard_loop_frequency = 100. self.pic_taking_speed = 10. self.waypoint_check_speed = 10. # Run this ROS node at the onboard loop frequency self.nutjobcase = rospy.Timer( rospy.Duration(1. / self.onboard_loop_frequency), self.run_process) #self.wedfsj = rospy.Timer(rospy.Duration(1. / self.pic_taking_speed), self.pic_initiator) self.wedfsj = rospy.Timer( rospy.Duration(1. / self.waypoint_check_speed), self.mission_planner) self.postcom = PositionController() def update_state(self, engage_command): if (engage_command.data == "start"): self.run_state = True elif (engage_command.data == "stop"): self.run_state = False #Publish Errors def publish_errors(self): msg = PoseStamped() msg.header.stamp = rospy.Time.now() msg.pose.position.x = self.translation_x_desired - self.translation_x msg.pose.position.y = self.translation_y_desired - self.translation_y msg.pose.position.z = self.translation_z_desired - self.translation_z euler_angle = euler_from_quaternion([ self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w ]) angle_yaw = euler_angle[2] msg.pose.orientation.x = angle_yaw self.pub_errors.publish(msg) #Controlling Process def process_commands(self): #Create Orientation Quaternion Arrays self.rotation = np.array([ self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w ]) self.rotation_desired = np.array([ self.rotation_x_desired, self.rotation_y_desired, self.rotation_z_desired, self.rotation_w_desired ]) #Calulate Time Interval from previous process current_time = rospy.get_time() self.time_interval = current_time - self.old_time self.old_time = current_time #Send Required Values to Position Controller self.postcom.update_pos_controller_values( self.translation_x, self.translation_y, self.translation_z, self.rotation, self.translation_x_desired, self.translation_y_desired, self.translation_z_desired, self.rotation_desired, self.time_interval) #Recieve Calculation of Commands returnvalue = self.postcom.calculate_commands() [roll, pitch, yaw, z_dot] = returnvalue #Publish Errors self.publish_errors() #Call function to publish commands self.set_vel(roll, pitch, z_dot, yaw) # Take desired position value def update_desired_position(self, desired_position_data): #Update Current Position self.translation_x_desired = desired_position_data.pose.position.x self.translation_y_desired = desired_position_data.pose.position.y self.translation_z_desired = desired_position_data.pose.position.z #Update Current Orientation self.rotation_x_desired = desired_position_data.pose.orientation.x self.rotation_y_desired = desired_position_data.pose.orientation.y self.rotation_z_desired = desired_position_data.pose.orientation.z self.rotation_w_desired = desired_position_data.pose.orientation.w # Obtain value from VICON def update_vicon(self, vicon_data_msg): #Update Current Position self.translation_x = vicon_data_msg.transform.translation.x self.translation_y = vicon_data_msg.transform.translation.y self.translation_z = vicon_data_msg.transform.translation.z #Update Current Orientation self.rotation_x = vicon_data_msg.transform.rotation.x self.rotation_y = vicon_data_msg.transform.rotation.y self.rotation_z = vicon_data_msg.transform.rotation.z self.rotation_w = vicon_data_msg.transform.rotation.w self.current_time = vicon_data_msg.header.stamp #Publish velocity to /cmd_vel_RHC def set_vel(self, roll, pitch, z_dot, yaw_dot): msg = Twist() msg.linear.x = roll msg.linear.y = pitch msg.linear.z = z_dot msg.angular.z = yaw_dot self.pub_vel.publish(msg) pass def run_process(self, random): if (self.run_state): self.process_commands() def pic_initiator(self, random): margin = 0.25 picture_point_x = self.pic_x[self.pic_counter] picture_point_y = self.pic_y[self.pic_counter] picture_point_z = self.pic_z[self.pic_counter] if (((picture_point_x - margin) < self.translation_x < (picture_point_x + margin)) and ((picture_point_y - margin) < self.translation_y < (picture_point_y + margin)) and ((picture_point_z - margin) < self.translation_z < (picture_point_z + margin))): capture_message = "capture" self.pic_counter = self.pic_counter + 1 self.request_pic.publish(capture_message) def mission_planner(self, random): margin = 0.25 x = self.order_pic[self.waypoint_counter] waypoint_x = self.x_position_array[x] waypoint_y = self.x_position_array[x] if (((waypoint_x - margin) < self.translation_x < (waypoint_x + margin)) and ((waypoint_y - margin) < self.translation_y < (waypoint_y + margin))): self.waypoint_counter = self.waypoint_counter + 1 if (self.waypoint_counter == len(self.order_pic)): end_message = "land" self.request_land.publish(end_message) print("reached")
class ROSControllerNode(object): #Initialize Ros Controller def __init__(self): self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32) self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_vicon) self.desired_position_data = rospy.Subscriber( '/desired_positions', PoseStamped, self.update_desired_position) self.pub_errors = rospy.Publisher('/errors', PoseStamped, queue_size=100) #self.pub_check = rospy.Publisher('/check_mate', String, queue_size = 100) self.translation_x_old = 0.0 self.translation_y_old = 0.0 self.translation_z_old = 0.0 self.z_velocity_old = 0.0 self.translation_x_desired = 0.0 self.translation_y_desired = 0.0 self.translation_z_desired = 0.0 self.translation_x = 0.0 self.translation_y = 0.0 self.translation_z = 0.0 self.rotation_x = 0.0 self.rotation_y = 0.0 self.rotation_z = 0.0 self.rotation_w = 1.0 self.rotation_x_desired = 0.0 self.rotation_y_desired = 0.0 self.rotation_z_desired = 0.0 self.rotation_w_desired = 1.0 self.time_interval = 0 self.current_time = rospy.get_time() self.old_time = rospy.get_time() self.postcom = PositionController() # Run the publish commands at 100 Hz self.onboard_loop_frequency = 100. # Run this ROS node at the onboard loop frequency self.nutjobcase = rospy.Timer( rospy.Duration(1. / self.onboard_loop_frequency), self.process_commands) self.postcom = PositionController() #Publish Errors def publish_errors(self): msg = PoseStamped() msg.header.stamp = rospy.Time.now() msg.pose.position.x = self.translation_x_desired - self.translation_x msg.pose.position.y = self.translation_y_desired - self.translation_y msg.pose.position.z = self.translation_z_desired - self.translation_z euler_angle = euler_from_quaternion([ self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w ]) angle_yaw = euler_angle[2] msg.pose.orientation.x = angle_yaw self.pub_errors.publish(msg) #Controlling Process def process_commands(self, random): #Create Orientation Quaternion Arrays self.rotation = np.array([ self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w ]) self.rotation_desired = np.array([ self.rotation_x_desired, self.rotation_y_desired, self.rotation_z_desired, self.rotation_w_desired ]) #Calulate Time Interval from previous process current_time = rospy.get_time() self.time_interval = current_time - self.old_time self.old_time = current_time #Send Required Values to Position Controller self.postcom.update_pos_controller_values( self.translation_x, self.translation_y, self.translation_z, self.rotation, self.translation_x_desired, self.translation_y_desired, self.translation_z_desired, self.rotation_desired, self.time_interval) #Recieve Calculation of Commands returnvalue = self.postcom.calculate_commands() """ roll = returnvalue[0] pitch = returnvalue[1] yaw = returnvalue[2] z_dot = returnvalue[3] """ [roll, pitch, yaw, z_dot] = returnvalue #Publish Errors self.publish_errors() #Call function to publish commands self.set_vel(roll, pitch, z_dot, yaw) # Take desired position value def update_desired_position(self, desired_position_data): #Update Current Position self.translation_x_desired = desired_position_data.pose.position.x self.translation_y_desired = desired_position_data.pose.position.y self.translation_z_desired = desired_position_data.pose.position.z #Update Current Orientation self.rotation_x_desired = desired_position_data.pose.orientation.x self.rotation_y_desired = desired_position_data.pose.orientation.y self.rotation_z_desired = desired_position_data.pose.orientation.z self.rotation_w_desired = desired_position_data.pose.orientation.w # Obtain value from VICON def update_vicon(self, vicon_data_msg): #Update Current Position self.translation_x = vicon_data_msg.transform.translation.x self.translation_y = vicon_data_msg.transform.translation.y self.translation_z = vicon_data_msg.transform.translation.z #Update Current Orientation self.rotation_x = vicon_data_msg.transform.rotation.x self.rotation_y = vicon_data_msg.transform.rotation.y self.rotation_z = vicon_data_msg.transform.rotation.z self.rotation_w = vicon_data_msg.transform.rotation.w self.current_time = vicon_data_msg.header.stamp #Publish velocity to /cmd_vel_RHC def set_vel(self, roll, pitch, z_dot, yaw_dot): msg = Twist() msg.linear.x = roll msg.linear.y = pitch msg.linear.z = z_dot msg.angular.z = yaw_dot self.pub_vel.publish(msg) pass
def __init__(self, **kwargs): self.mainloop = gobject.MainLoop() self.pipeline = gst.Pipeline("pipeline") self.verbose = kwargs.get('verbose', True) self.debug = kwargs.get('debug', False) cam_width = kwargs.get('cam_width', 640) cam_height = kwargs.get('cam_height', 480) host = kwargs.get('host', '127.0.0.1') port = kwargs.get('port', 5000) h264 = kwargs.get('h264', False) self.marker_spotted = False self.image_processing = ImageProcessing(area_threshold=10) self.state_estimate = StateEstimationAltitude() self.autopilot = AutoPilot(self.state_estimate) self.position_controller = PositionController( self.autopilot, self.state_estimate) if h264: self.videosrc = gst.parse_launch( 'uvch264_src device=/dev/video0 name=src auto-start=true src.vfsrc') else: self.videosrc = gst.element_factory_make('v4l2src', 'v4l2src') fps = 30 self.vfilter = gst.element_factory_make("capsfilter", "vfilter") self.vfilter.set_property('caps', gst.caps_from_string( 'image/jpeg, width=%s, height=%s, framerate=20/1' % (str(cam_width), str(cam_height)))) self.queue = gst.element_factory_make("queue", "queue") self.udpsink = gst.element_factory_make('udpsink', 'udpsink') self.rtpjpegpay = gst.element_factory_make('rtpjpegpay', 'rtpjpegpay') self.udpsink.set_property('host', host) self.udpsink.set_property('port', port) self.pipeline.add_many( self.videosrc, self.queue, self.vfilter, self.rtpjpegpay, self.udpsink) gst.element_link_many( self.videosrc, self.queue, self.vfilter, self.rtpjpegpay, self.udpsink) pad = next(self.queue.sink_pads()) # Sending frames to onVideBuffer where openCV can do processing. pad.add_buffer_probe(self.onVideoBuffer) self.pipeline.set_state(gst.STATE_PLAYING) self.i = 0 gobject.threads_init() context = self.mainloop.get_context() #previous_update = time.time() fpstime = time.time() while True: try: context.iteration(False) self.autopilot._read_sensors() if self.autopilot.auto_switch > 1700: self.position_controller.headingHold() self.position_controller.holdAltitude() self.autopilot.send_control_commands() else: self.position_controller.reset_targets() print self.autopilot.print_commands() except KeyboardInterrupt: fps = self.i / (time.time() - fpstime) print 'fps %f ' % fps self.autopilot.dump_log() self.autopilot.disconnect_from_drone()
#Print to screen using curses if col == 0: screen.addstr(line, 0, string) if col == 1: screen.addstr(line, 40, string) screen.refresh() logging.basicConfig(filename='flight_test.log', level=logging.DEBUG) #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) #Initalization of curses screen = curses.initscr() screen.clear() screen.refresh() plot.ion() switch = False timer = time.time() while (vidro.current_rc_channels[4] > 1600) and (flight_ready == True): controller.I_error_alt = 0 controller.I_error_pitch = 0
#print 'Num Objs: ',repr(num_objects),' Cx: ',repr(cx),' Cy: ',repr(cy),' Area: ',repr(area_max) #image_data = [cx ,cy, area, min_con_x, max_con_x, min_con_y, max_con_y, out_of_bounds, num_objects] image_data = [cx,cy,area_max,num_objects] return image_data def get_camera_frame(): global frame global new_frame _, frame = cap.read() frame = cv2.flip(frame,-1,-1) new_frame = True #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() previous_time = time.time() cycle = 0 controller.update_gains() sequence = 0 seq0_cnt = 0 seq1_cnt = 0 seq2_cnt = 0 seq3_cnt = 0 pos_bound_err = 150 yaw_bound_err = 0.2
def __init__(self, uav_ids, init, fin, vx_ds, vy_ds, vz_ds, yaw_ds): self.cf_ids = uav_ids self.number_of_agents = np.shape(uav_ids)[0] self.takoff_alt = 1.0 # change? self._pos = {} self._vel = {} self._quat = {} self._dist_to_goal = {} self._euler_angles = {} self._euler_angular_rates = {} self._z_old = {} self._z_oold = {} self.yaw_old = {} self.radius = 0.15 self.d_star = self.radius self.MaxVelo = 1.0 # Tune these self.c1 = 7 * 4.5 self.c2 = 9 * 4.5 self.RepulsiveGradient = 7.5 * 100 self.previous_time = 0.0 self.change_time = -100.0 self.controller = PositionController() self.init_time = rospy.get_time() ### Publish ### # waypoint messages self.goal_msg_0, self.goal_msg_1, self.goal_msg_2, self.goal_msg_3 = PoseStamped( ), PoseStamped(), PoseStamped(), PoseStamped() self.goal_msgs = { '0': self.goal_msg_0, '1': self.goal_msg_1, '2': self.goal_msg_2, '3': self.goal_msg_3 } # rotor velocities messages # self.cmdV_msg_0, self.cmdV_msg_1, self.cmdV_msg_2, self.cmdV_msg_3 = Actuators(), Actuators(), Actuators(), Actuators() # self.rotor_vel_msgs = { # '0' : self.cmdV_msg_0, # '1' : self.cmdV_msg_1, # '2' : self.cmdV_msg_2, # '3' : self.cmdV_msg_3 # } ### Subscribe ### # odometry messages self._currpos_callbacks = { '0': self._currpos_callback_0, '1': self._currpos_callback_1, '2': self._currpos_callback_2, '3': self._currpos_callback_3 } # set parameters self.initials, self.finals = {}, {} self.goal_pubs, self.cmdVtemp_pubs, self.cmdV_pubs, self.takeoffs, self.lands = {}, {}, {}, {}, {} for index, cf_id in enumerate(self.cf_ids): self.initials[str(cf_id)] = init[index][:] self._z_old[str(cf_id)] = 0.0 self._z_oold[str(cf_id)] = 0.0 self.yaw_old[str(cf_id)] = 0.0 self.finals[str(cf_id)] = fin[index][:] self.goal_pubs[str(cf_id)] = rospy.Publisher('/crazyflie2_' + str(cf_id) + '/goal', PoseStamped, queue_size=1) self.cmdV_pubs[str(cf_id)] = rospy.Publisher( '/crazyflie2_' + str(cf_id) + '/command/motor_speed', Actuators, queue_size=1) rospy.Subscriber("/crazyflie2_" + str(cf_id) + "/odometry", Odometry, self._currpos_callbacks[str(cf_id)]) self.vx_d = vx_ds self.vy_d = vy_ds self.vz_d = vz_ds self.yaw_d = yaw_ds self.takeoffed = False self.reached_1st = False self.flag = {'flying': 0, 'landed': 0, 'preland': 0} # self.vstate = { # 'takeoff' : self.do_takeoff, # 'wpnav' : self.do_wpnav, # 'land' : self.do_land # } # Position controller self.controller = PositionController()
return if line > 22 or line < 0: return #Print to screen using curses if col == 0: screen.addstr(line, 0, string) if col == 1: screen.addstr(line, 40, string) screen.refresh() vidro = Vidro(True) vidro.connect() controller = PositionController(vidro) screen = curses.initscr() screen.clear() screen.refresh() while vidro.current_rc_channels[4] > 1600: controller.update_gains() try: controller.rc_alt(500) #controller.rc_yaw(0) #controller.rc_xy(500,500) curses_print("No errors",2,0) except:
class ROSControllerNode(object): def __init__(self): """ROS interface for controlling the Parrot ARDrone in the Vicon Lab.""" self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32) #self.random_test = rospy.Publisher('/random_stuff', Twist, queue_size = 32) self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_vicon) #self.start_value = rospy.Subscriber('/start_execution', String, self.run_stuff) self.desired_position_data = rospy.Subscriber( '/desired_positions', PoseStamped, self.update_desired_position) self.pub_check = rospy.Publisher('/check_mate', String, queue_size=100) self.pub_counter = 0 self.translation_x_old = 0.0 self.translation_y_old = 0.0 self.translation_z_old = 0.0 self.z_velocity_old = 0.0 self.translation_x_desired = 0.0 self.translation_y_desired = 0.0 self.translation_z_desired = 0.5 self.translation_x = 0.0 self.translation_y = 0.0 self.translation_z = 0.0 self.rotation_x = 0.0 self.rotation_y = 0.0 self.rotation_z = 0.0 self.rotation_w = 1.0 self.rotation_x_desired = 0.0 self.rotation_y_desired = 0.0 self.rotation_z_desired = 0.0 self.rotation_w_desired = 1.0 self.time_interval = 0 self.current_time = rospy.get_time() self.old_time = rospy.get_time() self.postcom = PositionController() """ # Run the onboard controller at 50 Hz self.onboard_loop_frequency = 50. # Run this ROS node at the onboard loop frequency self.nutjobcase = rospy.Timer(rospy.Duration(1. / self.onboard_loop_frequency), self.process_commands) self.postcom = PositionController() """ """ new_message=String() new_message.data="true" self.pub_check.publish(new_message) """ def process_commands(self): self.rotation = np.array([ self.rotation_x, self.rotation_y, self.rotation_z, self.rotation_w ]) self.rotation_desired = np.array([ self.rotation_x_desired, self.rotation_y_desired, self.rotation_z_desired, self.rotation_w_desired ]) current_time = rospy.get_time() self.time_interval = current_time - self.old_time self.old_time = current_time #postcom = PositionController(self.translation_x_old,self.translation_y_old,self.translation_z_old,self.translation_x, self.translation_y, self.translation_z, self.rotation, self.translation_x_desired, self.translation_y_desired, self.translation_z_desired, self.rotation_desired, self.z_velocity_old,self.time_interval) self.postcom.update_pos_controller_values( self.translation_x, self.translation_y, self.translation_z, self.rotation, self.translation_x_desired, self.translation_y_desired, self.translation_z_desired, self.rotation_desired, self.time_interval) returnvalue = self.postcom.member() """ roll = returnvalue[0] pitch = returnvalue[1] yaw = returnvalue[2] z_dot = returnvalue[3] old_x = returnvalue[4] old_y = returnvalue[5] old_z = returnvalue[6] old_velocity_z = returnvalue[7] """ [roll, pitch, yaw, z_dot, old_x, old_y, old_z, old_velocity_z] = returnvalue yaw_dot = 0 self.set_vel(roll, pitch, z_dot, yaw) self.store_old_values(old_x, old_y, old_z, old_velocity_z) # Take desired position value def update_desired_position(self, desired_position_data): self.translation_x_desired = desired_position_data.pose.position.x self.translation_y_desired = desired_position_data.pose.position.y self.translation_z_desired = desired_position_data.pose.position.z self.rotation_x_desired = desired_position_data.pose.orientation.x self.rotation_y_desired = desired_position_data.pose.orientation.y self.rotation_z_desired = desired_position_data.pose.orientation.z self.rotation_w_desired = desired_position_data.pose.orientation.w self.run_stuff() self.process_commands() test_var = 0 """ while(test_var==0): if (((self.translation_x_desired - 0.10) < self.translation_x < (self.translation_x_desired + 0.10)) and ((self.translation_y_desired - 0.10) < self.translation_y < (self.translation_y_desired + 0.10)) and ((self.translation_z_desired - 0.10) < self.translation_z < (self.translation_z_desired + 0.10))): self.pub_check.publish("true") test_var=1 else: self.pub_check.publish("false") """ # Obtain value from VICON def update_vicon(self, vicon_data_msg): self.translation_x = vicon_data_msg.transform.translation.x self.translation_y = vicon_data_msg.transform.translation.y self.translation_z = vicon_data_msg.transform.translation.z self.rotation_x = vicon_data_msg.transform.rotation.x self.rotation_y = vicon_data_msg.transform.rotation.y self.rotation_z = vicon_data_msg.transform.rotation.z self.rotation_w = vicon_data_msg.transform.rotation.w self.current_time = vicon_data_msg.header.stamp def store_old_values(self, old_x, old_y, old_z, z_velocity_old): self.translation_x_old = old_x self.translation_y_old = old_y self.translation_z_old = old_z self.z_velocity_old = z_velocity_old def set_vel(self, roll, pitch, z_dot, yaw_dot): msg = Twist() msg.linear.x = roll msg.linear.y = pitch msg.linear.z = z_dot msg.angular.z = yaw_dot self.pub_vel.publish(msg) # 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 run_stuff(self): while (True): if (((self.translation_x_desired - 0.15) < self.translation_x < (self.translation_x_desired + 0.15)) and ((self.translation_y_desired - 0.15) < self.translation_y < (self.translation_y_desired + 0.15)) and ((self.translation_z_desired - 0.15) < self.translation_z < (self.translation_z_desired + 0.15))): self.pub_check.publish("true") break #else: #self.process_commands() #self.pub_check.publish("false") #time.sleep(5) pass
from vidro import Vidro, ViconStreamer from position_controller import PositionController import sys, math, time import socket, struct, threading import numpy as np import Adafruit_BBIO.ADC as ADC import logging ############################### #Setup of vidro and controller# ############################### vidro = Vidro(False, 1) flight_ready = vidro.connect_vicon() vidro.connect_mavlink() controller = PositionController(vidro) start_time = time.time() #Load gains controller.update_gains() #################### # Controller Values# #################### take_off_alt = 1000 # Pos Commands alt_com = 0 # Pos Values alt_pos = 0
def __init__(self, **kwargs): self.mainloop = gobject.MainLoop() self.pipeline = gst.Pipeline("pipeline") self.verbose = kwargs.get('verbose', True) self.debug = kwargs.get('debug', False) cam_width = kwargs.get('cam_width', 640) cam_height = kwargs.get('cam_height', 480) host = kwargs.get('host', '127.0.0.1') port = kwargs.get('port', 5000) h264 = kwargs.get('h264', False) self.marker_spotted = False self.image_processing = ImageProcessing(area_threshold=10) self.state_estimate = StateEstimationAltitude() self.autopilot = AutoPilot(self.state_estimate) self.position_controller = PositionController(self.autopilot, self.state_estimate) if h264: self.videosrc = gst.parse_launch( 'uvch264_src device=/dev/video0 name=src auto-start=true src.vfsrc' ) else: self.videosrc = gst.element_factory_make('v4l2src', 'v4l2src') fps = 30 self.vfilter = gst.element_factory_make("capsfilter", "vfilter") self.vfilter.set_property( 'caps', gst.caps_from_string( 'image/jpeg, width=%s, height=%s, framerate=20/1' % (str(cam_width), str(cam_height)))) self.queue = gst.element_factory_make("queue", "queue") self.udpsink = gst.element_factory_make('udpsink', 'udpsink') self.rtpjpegpay = gst.element_factory_make('rtpjpegpay', 'rtpjpegpay') self.udpsink.set_property('host', host) self.udpsink.set_property('port', port) self.pipeline.add_many(self.videosrc, self.queue, self.vfilter, self.rtpjpegpay, self.udpsink) gst.element_link_many(self.videosrc, self.queue, self.vfilter, self.rtpjpegpay, self.udpsink) pad = next(self.queue.sink_pads()) # Sending frames to onVideBuffer where openCV can do processing. pad.add_buffer_probe(self.onVideoBuffer) self.pipeline.set_state(gst.STATE_PLAYING) self.i = 0 gobject.threads_init() context = self.mainloop.get_context() #previous_update = time.time() fpstime = time.time() while True: try: context.iteration(False) self.autopilot._read_sensors() if self.autopilot.auto_switch > 1700: self.position_controller.headingHold() self.position_controller.holdAltitude() self.autopilot.send_control_commands() else: self.position_controller.reset_targets() print self.autopilot.print_commands() except KeyboardInterrupt: fps = self.i / (time.time() - fpstime) print 'fps %f ' % fps self.autopilot.dump_log() self.autopilot.disconnect_from_drone()
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)
#Filter for fencing values given by wand def filter_value(low,high,value): if value < low: value = low if value > high: value = high return value #Initialization of log logging.basicConfig(filename='demo.log', level=logging.DEBUG) #Creation of vidro and controller objects vidro = Vidro(False, 2) flight_ready = vidro.connect() controller = PositionController(vidro) #Setup the screen for curses screen = curses.initscr() screen.clear() screen.refresh() #switch for knowing if first time out of control loop switch = False #needed for live plotting plot.ion() #Setup of timer timer = time.time()
def __init__(self): self.pub_vel = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32) self.vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_vicon) self.desired_position_data = rospy.Subscriber( '/desired_positions', PoseStamped, self.update_desired_position) self.start_stop_command = rospy.Subscriber('/start_stop_toggle', String, self.update_state) self.request_pic = rospy.Publisher('/take_pic', String, queue_size=10) self.request_land = rospy.Publisher('/ask_land', String, queue_size=2) self.pub_errors = rospy.Publisher('/errors', PoseStamped, queue_size=100) #self.pub_check = rospy.Publisher('/check_mate', String, queue_size = 100) self.run_state = False self.translation_x_old = 0.0 self.translation_y_old = 0.0 self.translation_z_old = 0.0 self.z_velocity_old = 0.0 self.translation_x_desired = 0.0 self.translation_y_desired = 0.0 self.translation_z_desired = 0.0 self.translation_x = 0.0 self.translation_y = 0.0 self.translation_z = 0.0 self.rotation_x = 0.0 self.rotation_y = 0.0 self.rotation_z = 0.0 self.rotation_w = 1.0 self.rotation_x_desired = 0.0 self.rotation_y_desired = 0.0 self.rotation_z_desired = 0.0 self.rotation_w_desired = 1.0 self.pic_counter = 0 self.pic_x = [ 0, -2, -2, -2, -2, -2, -2, -2, -2, -2, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2 ] self.pic_y = [ 0, -2.0000, -1.5000, -1.0000, -0.5000, 0, 0.5000, 1.0000, 1.5000, 2.0000, 2.0000, 1.5000, 1.0000, 0.5000, 0, -0.5000, -1.0000, -1.5000, -2.0000, -2.0000, -1.5000, -1.0000, -0.5000, 0, 0.5000, 1.0000, 1.5000, 2.0000, 2.0000, 1.5000, 1.0000, 0.5000, 0, -0.5000, -1.0000, -1.5000, -2.0000, -2.0000, -1.5000, -1.0000, -0.5000, 0, 0.5000, 1.0000, 1.5000, 2.0000 ] self.pic_z = [ 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3, 1.3 ] self.waypoint_counter = 0 x = np.loadtxt( '/home/mason/aer1217/labs/src/aer1217_ardrone_simulator/scripts/order.txt', delimiter=None) self.order_pic = x.astype(int) self.x_position_array = [1, 4.26, 0.88, 4.33, 7.69, 1] self.y_position_array = [1, 1.23, 5.48, 8.04, 4.24, 1] self.time_interval = 0 self.current_time = rospy.get_time() self.old_time = rospy.get_time() self.postcom = PositionController() # Run the publish commands at 100 Hz self.onboard_loop_frequency = 100. self.pic_taking_speed = 10. self.waypoint_check_speed = 10. # Run this ROS node at the onboard loop frequency self.nutjobcase = rospy.Timer( rospy.Duration(1. / self.onboard_loop_frequency), self.run_process) #self.wedfsj = rospy.Timer(rospy.Duration(1. / self.pic_taking_speed), self.pic_initiator) self.wedfsj = rospy.Timer( rospy.Duration(1. / self.waypoint_check_speed), self.mission_planner) self.postcom = PositionController()
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 PositionControllerNode_ChihChun(object): """ROS interface for controlling the Crazyflie 2 in Gazebo.""" # write code here to define node publishers and subscribers # publish to /crazyflie2/command/motor_speed topic # subscribe to /crazyflie2/command/trajectory and /crazyflie2/odometry def __init__(self): # Publishers: rotor velocities self.pub_rotor_vel = rospy.Publisher('/crazyflie2/command/motor_speed', Actuators, queue_size=1) # Subscribers: desired posn, current posn #self._goal_msg = MultiDOFJointTrajectory() #self.sub_goal = rospy.Subscriber('crazyflie2/command/trajectory', MultiDOFJointTrajectory, self._goal_callback) self._currpos_msg = Odometry() self.sub_currpos = rospy.Subscriber('/crazyflie2/odometry', Odometry, self._currpos_callback) # end of publisher/subscriber # initialize other parameters self.data_list = [[ 'Time', 'Desired_x', 'Desired_y', 'Desired_z', 'Actual_x', 'Actual_y', 'Actual_z', 'Desired_yaw', 'Desired_roll', 'Desired_pitch', 'Desired_yaw_r', 'Desired_climb_r', 'Actual_roll', 'Actual_pitch', 'Actual_yaw_r', 'Actual_climb_r' ]] self.start = [1.0, 1.0] self.wp_id = 1 self.previous_time = 0.0 self.change_time = -100.0 self.controller = PositionController() self.init_time = rospy.get_time() # initialize position and roll, pitch, yaw self._x = 0 self._y = 0 self._z = 0 self.roll = 0 self.pitch = 0 self.yaw = 0 self._z_old = 0.0 self._z_oold = 0.0 self.yaw_old = 0.0 def set_rotor_vel(self, pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch, yaw, thrust): # change arg names? # get conversions rotorvel_converter = roll_pitch_yawrate_thrust_crazyflie( pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch, yaw, thrust) rotor_velocities = rotorvel_converter.CalculateRotorVelocities() # publish rotor velocities to Actuator msg = Actuators() msg.angular_velocities = rotor_velocities msg.header.stamp = self._currpos_msg.header.stamp self.pub_rotor_vel.publish(msg) def _currpos_callback(self, msg): self._currpos_msg = msg def get_data(self): self._z_oold = self._z_old self._z_old = self._z self.yaw_old = self.yaw # self._pos = self._vicon_msg.transform.translation self._x = self._currpos_msg.pose.pose.position.x self._y = self._currpos_msg.pose.pose.position.y self._z = self._currpos_msg.pose.pose.position.z # Convert to quaternion object for use by euler_from_quaternion() quaternion = np.array([ self._currpos_msg.pose.pose.orientation.x, self._currpos_msg.pose.pose.orientation.y, self._currpos_msg.pose.pose.orientation.z, self._currpos_msg.pose.pose.orientation.w ]) # Determine the euler angles euler = euler_from_quaternion(quaternion) # change? can i use this? self.roll = euler[0] self.pitch = euler[1] self.yaw = euler[2] # Determine the euler angular rates p,q,r and thrust self.p = self._currpos_msg.twist.twist.angular.x self.q = self._currpos_msg.twist.twist.angular.y self.r = self._currpos_msg.twist.twist.angular.z #self.thrust = self._currpos_msg.twist.twist.linear.z self.x_d = 1.0 self.y_d = 1.0 self.z_d = 1.0 self.yaw_d = 0.0 self.vx_d = 0.0 self.vy_d = 0.0 self.vz_d = 0.0 # desired position #self.x_d = self._goal_msg.points.transforms.translation.x #self.y_d = self._goal_msg.points.transforms.translation.y #self.z_d = self._goal_msg.points.transforms.translation.z # desired orientation #self.quat = np.array([self._goal_msg.points.transforms.rotation.x, # self._goal_msg.points.transforms.rotation.y, # self._goal_msg.points.transforms.rotation.z, # self._goal_msg.points.transforms.rotation.w]) #euler = euler_from_quaternion(self.quat) #self.yaw_d = euler[2] # desired velocities #self.vx_d = self._goal_msg.points.velocities.linear.x # may need to change in future #self.vy_d = self._goal_msg.points.velocities.linear.y #self.vz_d = self._goal_msg.points.velocities.linear.z def iteration(self, event): current_time = rospy.get_time() - self.init_time dt = current_time - self.previous_time self.previous_time = current_time #print(dt) if dt == 0: dt = 0.01 act_climb_r = (self._z - 2 * self._z_old + self._z_oold) / (dt**2) print("z_dd_real: ", act_climb_r) #act_yaw_r = (self.yaw - self.yaw_old)/dt self.get_data() roll_c, pitch_c, z_dot_c, yaw_dot_c = self.controller.get_command( self._x, self._y, self._z, self.roll, self.pitch, self.yaw, self.x_d, self.y_d, self.z_d, self.vx_d, self.vy_d, self.vz_d, self.yaw_d) #self.set_vel(roll_c, pitch_c, z_dot_c, yaw_dot_c) self.thrust_c = z_dot_c #+ 12000 print("thrust: ", z_dot_c) self.set_rotor_vel(pitch_c, roll_c, yaw_dot_c, self.p, self.q, self.r, self.roll, self.pitch, self.yaw, self.thrust_c)
plot_camera_time.append(current_time-start_time) plot_camera_cycle.append(current_time-previous_time) plot_camera_cycle_number.append(cycle) cycle += 1 previous_time = current_time curses_print("In camera loop",3,0) camera.capture(stream, 'bgr', use_video_port=True) # stream.array now contains the image data in BGR order frame = stream.array new_frame = True stream.truncate(0) #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) event = threading.Event() d = threading.Thread(name='camera', target=camera_thread, args=(event,)) d.start() start_time = time.time() previous_time = time.time() cycle = 0 controller.update_gains() sequence = 0
return image_data def get_camera_frame(): global frame global new_frame _, frame = cap.read() frame = cv2.flip(frame,-1,-1) new_frame = True ############################### #Setup of vidro and controller# ############################### vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() # Load gains controller.update_gains() #################### # Controller Values# #################### take_off_alt = 1000 # Pos Commands x_com = 0 y_com = 0 alt_com = 0 yaw_com = 0 # Pos Values
class position_controller_flock_node(object): """ROS interface for controlling up to four Cf2.0's in Gazebo and running flocking algorithm.""" def __init__(self, uav_ids, init, fin, vx_ds, vy_ds, vz_ds, yaw_ds): self.cf_ids = uav_ids self.number_of_agents = np.shape(uav_ids)[0] self.takoff_alt = 1.0 # change? self._pos = {} self._vel = {} self._quat = {} self._dist_to_goal = {} self._euler_angles = {} self._euler_angular_rates = {} self._z_old = {} self._z_oold = {} self.yaw_old = {} self.radius = 0.15 self.d_star = self.radius self.MaxVelo = 1.0 # Tune these self.c1 = 7 * 4.5 self.c2 = 9 * 4.5 self.RepulsiveGradient = 7.5 * 100 self.previous_time = 0.0 self.change_time = -100.0 self.controller = PositionController() self.init_time = rospy.get_time() ### Publish ### # waypoint messages self.goal_msg_0, self.goal_msg_1, self.goal_msg_2, self.goal_msg_3 = PoseStamped( ), PoseStamped(), PoseStamped(), PoseStamped() self.goal_msgs = { '0': self.goal_msg_0, '1': self.goal_msg_1, '2': self.goal_msg_2, '3': self.goal_msg_3 } # rotor velocities messages # self.cmdV_msg_0, self.cmdV_msg_1, self.cmdV_msg_2, self.cmdV_msg_3 = Actuators(), Actuators(), Actuators(), Actuators() # self.rotor_vel_msgs = { # '0' : self.cmdV_msg_0, # '1' : self.cmdV_msg_1, # '2' : self.cmdV_msg_2, # '3' : self.cmdV_msg_3 # } ### Subscribe ### # odometry messages self._currpos_callbacks = { '0': self._currpos_callback_0, '1': self._currpos_callback_1, '2': self._currpos_callback_2, '3': self._currpos_callback_3 } # set parameters self.initials, self.finals = {}, {} self.goal_pubs, self.cmdVtemp_pubs, self.cmdV_pubs, self.takeoffs, self.lands = {}, {}, {}, {}, {} for index, cf_id in enumerate(self.cf_ids): self.initials[str(cf_id)] = init[index][:] self._z_old[str(cf_id)] = 0.0 self._z_oold[str(cf_id)] = 0.0 self.yaw_old[str(cf_id)] = 0.0 self.finals[str(cf_id)] = fin[index][:] self.goal_pubs[str(cf_id)] = rospy.Publisher('/crazyflie2_' + str(cf_id) + '/goal', PoseStamped, queue_size=1) self.cmdV_pubs[str(cf_id)] = rospy.Publisher( '/crazyflie2_' + str(cf_id) + '/command/motor_speed', Actuators, queue_size=1) rospy.Subscriber("/crazyflie2_" + str(cf_id) + "/odometry", Odometry, self._currpos_callbacks[str(cf_id)]) self.vx_d = vx_ds self.vy_d = vy_ds self.vz_d = vz_ds self.yaw_d = yaw_ds self.takeoffed = False self.reached_1st = False self.flag = {'flying': 0, 'landed': 0, 'preland': 0} # self.vstate = { # 'takeoff' : self.do_takeoff, # 'wpnav' : self.do_wpnav, # 'land' : self.do_land # } # Position controller self.controller = PositionController() def set_rotor_vel(self, pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch, yaw, thrust): # change arg names? # get conversions rotorvel_converter = roll_pitch_yawrate_thrust_crazyflie( pitch_c, roll_c, yaw_rate_c, p, q, r, roll, pitch, yaw, thrust) rotor_velocities = rotorvel_converter.CalculateRotorVelocities() # publish rotor velocities to Actuator msg = Actuators() msg.angular_velocities = rotor_velocities msg.header.stamp = self._currpos_msg.header.stamp self.pub_rotor_vel.publish(msg) def _currpos_callback(self, msg): self._currpos_msg = msg def get_data(self, id): self._z_oold[str(id)] = self._z_old[str(id)] self._z_old[str(id)] = self._pos[str(id)].z # current z print("self._euler_angles[str(id)]: ", self._euler_angles[str(id)]) self.yaw_old[str(id)] = self._euler_angles[str(id)][2] # current yaw def _currpos_callback_0(self, data): self._pos['0'] = data.pose.pose.position self._vel['0'] = data.twist.twist.linear self._quat['0'] = np.array([ data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w ]) # gives quaternion object #print("quat 0: ", self._quat['0']) self._euler_angles['0'] = euler_from_quaternion( self._quat['0']) # gives roll, pitch, yaw #print("euler angles 0: ", self._euler_angles['0']) self._euler_angular_rates['0'] = np.asarray( data.twist.twist.angular) # gives p, q, r def _currpos_callback_1(self, data): self._pos['1'] = data.pose.pose.position self._vel['1'] = data.twist.twist.linear self._quat['1'] = np.array([ data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w ]) # gives quaternion object self._euler_angles['1'] = euler_from_quaternion( self._quat['1']) # gives roll, pitch, yaw #print(self._euler_angles['1'], type(self._euler_angles['1'])) self._euler_angular_rates['1'] = np.asarray( data.twist.twist.angular) # gives p, q, r def _currpos_callback_2(self, data): self._pos['2'] = data.pose.pose.position self._vel['2'] = data.twist.twist.linear self._quat['2'] = np.array([ data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w ]) # gives quaternion object self._euler_angles['2'] = euler_from_quaternion( self._quat['2']) # gives roll, pitch, yaw self._euler_angular_rates['2'] = np.asarray( data.twist.twist.angular) # gives p, q, r def _currpos_callback_3(self, data): self._pos['3'] = data.pose.pose.position self._vel['3'] = data.twist.twist.linear self._quat['3'] = np.array([ data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w ]) # gives quaternion object self._euler_angles['3'] = euler_from_quaternion( self._quat['3']) # gives roll, pitch, yaw self._euler_angular_rates['3'] = np.asarray( data.twist.twist.angular) # gives p, q, r def update_pos(self, id, pos): self.goal_msgs[id].header.seq += 1 self.goal_msgs[id].header.frame_id = '/world' # change? self.goal_msgs[id].header.stamp = rospy.Time.now() self.goal_msgs[id].pose.position.x = pos[0] self.goal_msgs[id].pose.position.y = pos[1] self.goal_msgs[id].pose.position.z = pos[2] self.goal_msgs[id].pose.orientation.x = 0 self.goal_msgs[id].pose.orientation.y = 0 self.goal_msgs[id].pose.orientation.z = 0 self.goal_msgs[id].pose.orientation.w = 1 def update_rotor_vels(self, id): # this function takes in the UAV's id and computes+publishes the rotor velocities to that UAV # get z_oold, z_old, and desired position/yaw self.get_data(id) # get roll/pitch/yawrate/thrust commands from position controller roll_c, pitch_c, z_dot_c, yaw_dot_c = self.controller.get_command( self._pos[str(id)].x, self._pos[str(id)].y, self._pos[str(id)].z, # x,y,z self._euler_angles[str(id)][0], self._euler_angles[str(id)][1], self._euler_angles[str(id)][0][2], # change? roll, pitch, yaw self.initials[str(id)][0], self.initials[str(id)][1], self.initials[str(id)][2], # change? xd, yd, zd self.vx_d[int(id)], self.vy_d[int(id)], self.vz_d[int(id)], self.yaw_d[int(id)]) # obtain p,q,r/roll,pitch,yaw for UAV id from odometry subscription p = self._euler_angular_rates[str(id)][0] q = self._euler_angular_rates[str(id)][1] r = self._euler_angular_rates[str(id)][2] roll = self._euler_angles[str(id)][0] pitch = self._euler_angles[str(id)][1] yaw = self._euler_angles[str(id)][2] # convert above commands to rotor velocity commands rotorvel_converter = roll_pitch_yawrate_thrust_crazyflie( pitch_c, roll_c, yaw_dot_c, p, q, r, roll, pitch, yaw, z_dot_c) rotor_velocities = rotorvel_converter.CalculateRotorVelocities( ) # this yields a 4-element list # publish rotor velocities to Actuator rotorvel_msg = Actuators() rotorvel_msg.angular_velocities = rotor_velocities #rotorvel_msg.header.stamp = self._currpos_msg.header.stamp self.cmdV_pubs[str(cf_id)].publish(rotorvel_msg) def publish_msg(self): for cf_id in self.cf_ids: self.goal_pubs[str(cf_id)].publish(self.goal_msgs[str(cf_id)]) def do_wpnav(self): print('Navigating!!') # Check all cfs reached assigened takeoff alt if not self.takeoffed: count = 0 for cf_id in self.cf_ids: if abs(self._pos[str(cf_id)][2] - self.takoff_alt) < 0.05: count += 1 position_d = np.array([ self._pos[str(cf_id)][0], self._pos[str(cf_id)][1], self.takoff_alt ]) self.update_pos(str(cf_id), position_d) if count == self.number_of_agents: self.takeoffed = True # Check all cfs reached their initial points elif not self.reached_1st: count = 0 for cf_id in self.cf_ids: if np.linalg.norm(self._pos[str(cf_id)] - self.initials[str(cf_id)]) < 0.1: count += 1 position_d = np.array([ self.initials[str(cf_id)][0], self.initials[str(cf_id)][1], self.initials[str(cf_id)][2] ]) self.update_pos(str(cf_id), position_d) if count == self.number_of_agents: self.reached_1st = True print('Initial points reached!!') raw_input( "Press Enter to continue...") # use input() for python3 # flocking path planning else: # call flocking and get the desired position of next timestep for cf_id in self.cf_ids: other_cfs = self.cf_ids[:] other_cfs.remove(cf_id) self._dist_to_goal[str(cf_id)] = np.linalg.norm( self._pos[str(cf_id)] - self.finals[str(cf_id)]) force = -self.c1 * self._vel[str(cf_id)] - self.c2 * ( self._pos[str(cf_id)] - self.finals[str(cf_id)]) for other_cf in other_cfs: dist_v = self._pos[str(other_cf)] - self._pos[str(cf_id)] dist = np.linalg.norm(dist_v) d = 2 * self.radius + self.d_star CommunicationRadious = np.cbrt( 3 * np.square(self.MaxVelo) / (2 * self.RepulsiveGradient)) + d if dist < CommunicationRadious: ForceComponent = -self.RepulsiveGradient * np.square( dist - CommunicationRadious) force += ForceComponent * (dist_v) / dist velocity_d = self._vel[str(cf_id)] + force * self.dt position_d = self._pos[str(cf_id)] + velocity_d * self.dt position_d[2] = 1.0 # for 2D sim self.update_pos(str(cf_id), position_d) # Check all cfs reached their final points if self._dist_to_goal[max(self._dist_to_goal)] < 0.2: self.flag['preland'] = 1 def iteration(self, event): # publish goal messages for each uav self.publish_msg() # publish rotor velocities for each uav for index, cf_id in enumerate(self.cf_ids): self.update_rotor_vels(cf_id)
#!/usr/bin/env python # -*- coding: utf-8 -*- from vidro import Vidro, ViconStreamer from position_controller import PositionController import sys, math, time import socket, struct, threading import numpy as np import cv2 #Setup of vidro and controller vidro = Vidro(False, 1) flight_ready = vidro.connect() controller = PositionController(vidro) start_time = time.time() previous_time = time.time() cycle = 0 controller.update_gains() sequence = 0 seq0_cnt = 0 seq1_cnt = 0 seq2_cnt = 0 seq3_cnt = 0 pos_bound_err = 300 yaw_bound_err = 0.5 yaw = 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): self.position_controller = PositionController() self.sub_desired_state = rospy.Subscriber( '/aer1217_ardrone/desired_state', DesiredStateMsg, self.update_quadrotor_desired_state) ####################################################################### # input is the estimated state x,y,z,roll,pitch,yaw from VICON self.sub_estimated_state_vicon = rospy.Subscriber( '/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_quadrotor_estimated_state_from_vicon) ####################################################################### # output roll,pitch,yaw_rate,climb_rate commands self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=30) self.pub_cmd_vel_RHC = rospy.Publisher('cmd_vel_RHC', Twist, queue_size=30) ####################################################################### self.onboard_loop_frequency = 200. self.pub_cmd_vel_timer = rospy.Timer( rospy.Duration(1. / self.onboard_loop_frequency), self.send_cmd_vel) def update_quadrotor_desired_state(self, desired_state_msg): self.position_controller.set_desired_state( desired_state_msg.x[0], desired_state_msg.x[1], desired_state_msg.x[2], desired_state_msg.x_dot[0], desired_state_msg.x_dot[1], desired_state_msg.x_dot[2], desired_state_msg.x_2dot[0], desired_state_msg.x_2dot[1], desired_state_msg.x_2dot[2], desired_state_msg.yaw, desired_state_msg.yaw_rate) def update_quadrotor_estimated_state_from_vicon(self, estimated_state_msg): self.estimated_state = estimated_state_msg x = estimated_state_msg.transform.translation.x y = estimated_state_msg.transform.translation.y z = estimated_state_msg.transform.translation.z a = estimated_state_msg.transform.rotation.x b = estimated_state_msg.transform.rotation.y c = estimated_state_msg.transform.rotation.z d = estimated_state_msg.transform.rotation.w (roll, pitch, yaw) = euler_from_quaternion([a, b, c, d]) self.position_controller.set_estimated_state(x, y, z, roll, pitch, yaw) def send_cmd_vel(self, event): # output roll,pitch,yaw_rate,climb_rate commands (pitch, roll, yaw_rate, climb_rate ) = self.position_controller.get_pitch_roll_yawrate_climbrate() cmd_vel = Twist() cmd_vel.linear.x = roll #roll angle in radian cmd_vel.linear.y = pitch #pitch angle in radian cmd_vel.linear.z = climb_rate #climb rate cmd_vel.angular.x = 0 cmd_vel.angular.y = 0 cmd_vel.angular.z = yaw_rate #raw rate in radian per second self.pub_cmd_vel.publish(cmd_vel) self.pub_cmd_vel_RHC.publish(cmd_vel)
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): # Publishers self.pub_vel_cmd = rospy.Publisher('/cmd_vel_RHC', Twist, queue_size=32) self.pub_error = rospy.Publisher('/aer1217/position_error', Twist, queue_size=32) #Subscriber self.sub_vicon = rospy.Subscriber('/vicon/ARDroneCarre/ARDroneCarre', TransformStamped, self.update_quadrotor_state) self.sub_desired_pos = rospy.Subscriber('/aer1217/desired_position', TransformStamped, self.update_desired_pos) #initialize variables self.actual_pos = TransformStamped() self.actual_pos_prev = TransformStamped() self.actual_vel = np.array([0, 0, 0, 0]) self.desired_pos = TransformStamped() self.desired_pos_prev = TransformStamped() self.desired_vel = np.array([0, 0, 2, 0]) #Define Published msgs self.pos_error = Twist() self.vel_cmd_msg = Twist() #initialize position controller self.pos_controller = PositionController() #publish vel commands self.dt = 1 / 100.0 self.pub_prop_vel = rospy.Timer(rospy.Duration(self.dt), \ self.send_vel_cmd) def update_quadrotor_state(self, transfrom_stamped_msg): """Callback function for sub_vicon topic, stores the quad rotor position and also calculates its velocity Args: tranform_stamped_msg: msg received from subscriber """ #store received msg self.actual_pos = transfrom_stamped_msg #calculate velocity rotation = (self.actual_pos.transform.rotation.x, self.actual_pos.transform.rotation.y, self.actual_pos.transform.rotation.z, self.actual_pos.transform.rotation.w) euler_angle = euler_from_quaternion(rotation) psi_act = euler_angle[2] pos_act = np.array([ self.actual_pos.transform.translation.x, self.actual_pos.transform.translation.y, self.actual_pos.transform.translation.z, psi_act ]) t_act = self.actual_pos.header.stamp.to_sec() + \ self.actual_pos.header.stamp.to_nsec()/(10**9) rotation = (self.actual_pos_prev.transform.rotation.x, self.actual_pos_prev.transform.rotation.y, self.actual_pos_prev.transform.rotation.z, self.actual_pos_prev.transform.rotation.w) euler_angle = euler_from_quaternion(rotation) psi_prev = euler_angle[2] pos_prev = np.array([ self.actual_pos_prev.transform.translation.x, self.actual_pos_prev.transform.translation.y, self.actual_pos_prev.transform.translation.z, psi_prev ]) t_prev = self.actual_pos_prev.header.stamp.to_sec() + \ self.actual_pos_prev.header.stamp.to_nsec()/(10**9) try: delta = pos_act - pos_prev #Ensure difference in yaw doesn't exceed pi if delta[3] > np.pi: delta[3] = delta[3] - 2 * np.pi elif delta[3] < -np.pi: delta[3] = delta[3] + 2 * np.pi #calculate velocity self.actual_vel = delta / float(t_act - t_prev) except: ropsy.logwarn('Division by zero encountered when calculating \ actual velocity') pass self.actual_pos_prev = self.actual_pos def update_desired_pos(self, transformstamped_msg): """Callback function for sub_desired_pos topic, stores the desired position and also calculates its velocity Args: tranformstamped_msg: msg received from subscriber """ #store desired position self.desired_pos = transformstamped_msg pos_act = np.array([ self.desired_pos.transform.translation.x, self.desired_pos.transform.translation.y, self.desired_pos.transform.translation.z, self.desired_pos.transform.rotation.z ]) tdes_act = self.desired_pos.header.stamp.to_sec() + \ self.desired_pos.header.stamp.to_nsec()/(10**9) pos_prev = np.array([ self.desired_pos_prev.transform.translation.x, self.desired_pos_prev.transform.translation.y, self.desired_pos_prev.transform.translation.z, self.desired_pos_prev.transform.rotation.z ]) tdes_prev = self.desired_pos_prev.header.stamp.to_sec() + \ self.desired_pos_prev.header.stamp.to_nsec()/(10**9) try: delta = pos_act - pos_prev #Ensure difference in yaw doesn't exceed pi if delta[3] > np.pi: delta[3] = delta[3] - 2 * np.pi elif delta[3] < -np.pi: delta[3] = delta[3] + 2 * np.pi #calculate velocity self.desired_vel = delta / float(tdes_act - tdes_prev) except: ropsy.logwarn('Division by zero encountered when calculating \ desired velocity') pass self.desired_pos_prev = self.desired_pos def send_vel_cmd(self, event): """Function that sends velocity commands to ardrone """ #calculate commands using position controller phi_c, theta_c, z_acc, psi_acc = self.pos_controller.pos_cont(self.actual_pos, \ self.desired_pos, self.actual_vel, self.desired_vel) #set velocity commands self.vel_cmd_msg.linear.x = phi_c self.vel_cmd_msg.linear.y = theta_c self.vel_cmd_msg.linear.z = np.clip( self.vel_cmd_msg.linear.z + z_acc * self.dt, -1.0, 1.0) self.vel_cmd_msg.angular.z = np.clip( self.vel_cmd_msg.angular.z + psi_acc * self.dt, -1000.0, 1000.0) #record position error self.pos_error.linear.x = self.desired_pos.transform.translation.x - \ self.actual_pos.transform.translation.x self.pos_error.linear.y = self.desired_pos.transform.translation.y - \ self.actual_pos.transform.translation.y self.pos_error.linear.z = self.desired_pos.transform.translation.z - \ self.actual_pos.transform.translation.z rotation = (self.actual_pos.transform.rotation.x, self.actual_pos.transform.rotation.y, self.actual_pos.transform.rotation.z, self.actual_pos.transform.rotation.w) euler_angle = euler_from_quaternion(rotation) psi_act = euler_angle[2] print('yaw = ', psi_act, 'yaw_error=', self.desired_pos.transform.rotation.z - psi_act) #Publish position error self.pub_error.publish(self.pos_error) #publish velocity commands self.pub_vel_cmd.publish(self.vel_cmd_msg)
return euler_from_quaternion(orient_q) def get_time(self): return self._vicon_msg.header.stamp.nsecs def land(self): self.pub_land.publish() def set_vel(self, traj): self.pub_traj.publish(traj) if __name__ == '__main__': # write code to create ROSControllerNode rospy.init_node("ros_interface", disable_signals=True) positionCtrl = PositionController() ardrone = ROSControllerNode() # ardrone.time_stamp = ardrone.get_time() #first test # x_des = 4 # y_des = 4 # z_des = 4 # yaw_des = 1 try: while not rospy.is_shutdown(): #get position nad orientation from vicon currentPosition = ardrone.get_pos() currentOrientation = ardrone.get_orient() #compute desired pose