def timer_callback(self, event): #print(self.alt_height) # Pass wished yaw position (must be between -1 and 1) and measured yaw _, _, set_y = get_rpy_orientation( self.current_uav_setpoint.orientation) self.yaw_setpoint_pub.publish(set_y) self.yaw_state_pub.publish(self.yaw) # Pass wished altitude and measured altitude self.altitude_setpoint_pub.publish( self.current_uav_setpoint.position.z) self.altitude_state_pub.publish(self.alt_height) # Pass wished alt-rate and estimated speed self.altitude_rate_setpoint_pub.publish(self.alt_effort) #self.altitude_rate_setpoint_pub.publish(self.current_uav_setpoint.position.z) self.altitude_rate_state_pub.publish(self.alt_est_speed) rpyt = RollPitchYawrateThrust() # Define standard thrust, this should be roughly when the drone is standing still in the air. thrust_ref = 7.1125 output = thrust_ref + self.alt_rate_effort # Pass thrust to uav rpyt.thrust.z = thrust_ref + self.alt_rate_effort # Pass pitch to uav rpyt.pitch = self.current_uav_setpoint.orientation.y # Pass roll to uav rpyt.roll = self.current_uav_setpoint.orientation.x # Pass yaw-rate to uav rpyt.yaw_rate = self.yaw_effort self.rpyt_pub.publish(rpyt)
def sendCommand(self): msg = RollPitchYawrateThrust() msg.roll = self.cmd[0] msg.pitch = self.cmd[1] msg.yaw_rate = self.cmd[2] msg.thrust.z = self.cmd[3] self.pubcmd.publish(msg)
def pidaltitude(self, data): # We wish that our drones current position error is 0, the setpoint is the distance to the object / position we- # want to go to therefore we publish 0 to the PID state. self.PIDyaw_state.publish(0) self.PIDpitch_state.publish(0) self.PIDroll_state.publish(0) rotor_speed = RollPitchYawrateThrust() rotor_speed.roll = self.roll_control_effort.data rotor_speed.yaw_rate = self.yaw_control_effort.data rotor_speed.pitch = self.pitch_control_effort.data rotor_speed.thrust.z = float(data.data / 10.99934) + 7.31 self.humm_rotor_pub.publish(rotor_speed)
def timer_callback(self, event): ''' Pass the variables to the relevant publishers.self yaw PID: input.orientation.z -> setpoint measured_yaw -> state Altitude PID: input.position.z (how high we want to go) -> setpoint measured_height -> state Altitude-rate PID: altitude_PID_output -> setpoint estimated_speed -> state RollPitchYawrateThrust (input to UAV): input.pitch -> pitch input.roll -> roll yaw_PID_output -> yaw-rate altitude_rate_PID_output -> thrust ''' # Pass wished yaw position (must be between -1 and 1) and measured yaw self.yaw_setpoint_pub.publish(self.current_uav_setpoint.orientation.z) self.yaw_state_pub.publish(self.yaw) # Pass wished altitude and measured altitude self.altitude_setpoint_pub.publish( self.current_uav_setpoint.position.z) self.altitude_state_pub.publish(self.alt_height) # Pass wished alt-rate and estimated speed self.altitude_rate_setpoint_pub.publish(self.alt_effort) #self.altitude_rate_setpoint_pub.publish(self.current_uav_setpoint.position.z) self.altitude_rate_state_pub.publish(self.alt_est_speed) rpyt = RollPitchYawrateThrust() # Define standard thrust, this should be roughly when the drone is standing still in the air. thrust_ref = 7.11 output = thrust_ref + self.alt_rate_effort # Pass thrust to uav rpyt.thrust.z = thrust_ref + self.alt_rate_effort # Pass pitch to uav rpyt.pitch = self.current_uav_setpoint.orientation.y # Pass roll to uav rpyt.roll = self.current_uav_setpoint.orientation.x # Pass yaw-rate to uav rpyt.yaw_rate = self.yaw_effort self.rpyt_pub.publish(rpyt)
def callbackCmdVel(self, data): if self.pub is None: return vel = data cmd = RollPitchYawrateThrust() cmd.header.stamp = rospy.Time.now() # cmd.header.frame_id = "X3/base_link" cmd.roll = -vel.linear.y cmd.pitch = vel.linear.x cmd.yaw_rate = vel.angular.z cmd.thrust.z = vel.linear.z self.pub.publish(cmd)
def __init__(self, namespace): self.namespace = namespace self.start_time = rospy.Time.now() self.got_odom = False self.pose_x = [] self.pose_y = [] self.pose_z = [] # self.waypoint = MultiDOFJointTrajectoryPoint() self.action = RollPitchYawrateThrust self.active = 'None' self.got_dnn = False self.got_mpc = False self.restarting = False self.dnn = RollPitchYawrateThrust() self.mpc = RollPitchYawrateThrust() self.dagger_beta = 1.0 waypoint_string = '/' + str(self.namespace) + '/command/trajectory' selection_string = '/' + str(self.namespace + '/dagger/roll_pitch_yawrate_thrust') odom_string = '/' + str(self.namespace) + '/ground_truth/odometry' dnn_string = '/' + str( self.namespace) + '/dnn/roll_pitch_yawrate_thrust' mpc_string = '/' + str( self.namespace) + '/command/roll_pitch_yawrate_thrust' # self.waypoint_pub = rospy.Publisher(waypoint_string, Rol, queue_size=5) self.selection_pub = rospy.Publisher(selection_string, RollPitchYawrateThrust, queue_size=5) self.odom_sub = rospy.Subscriber(odom_string, Odometry, self.callback_odom, queue_size=10) self.dnn_sub = rospy.Subscriber(dnn_string, RollPitchYawrateThrust, self.callback_dnn, queue_size=1) self.mpc_sub = rospy.Subscriber(mpc_string, RollPitchYawrateThrust, self.callback_astar, queue_size=1)
def onClose(self): self.logfile.close() print "Closing..." self.enabled = False rpytmsg = RollPitchYawrateThrust() self.pubcmd.publish(rpytmsg) set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) msmsg = ModelState() msmsg.model_name = 'firefly' msmsg.reference_frame = 'world' try: rep = set_model_state(msmsg) print 'Model Reset' except rospy.ServiceException as exc: print 'Service Error:' + str(exc) self.pubcmd.publish(rpytmsg)
def main(): print "Calibration uwp" rospy.init_node('calibration_unity') status = 0 roll = 0.0 pitch = 0.0 yaw = 0.0 thrust = Vector3() rpy = RollPitchYawrateThrust() while (1): key = getKey() status = status + 1 if key == 'pageup': roll = roll + 0.1 elif key == 'pagedown': roll = roll - 0.1 elif key == 'up': pitch = pitch + 0.1 elif key == 'down': pitch = pitch - 0.1 elif key == 'right': yaw = yaw + 0.1 elif key == 'left': yaw = yaw - 0.1 else: if (key == '\x03'): break if status == 20: print roll + ", " + pitch + ", " + yaw status = 0 rpy.roll = roll rpy.pitch = pitch rpy.yaw_rate = yaw rpy.thrust = thrust pub.publish(rpy)
def callback_image(self, data): cv_image = [] try: np_arr = np.fromstring(data.data, np.uint8) cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) # cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB) # text_for_image = 'DNN Image Input' # font = cv2.FONT_HERSHEY_SIMPLEX # cv2.putText(cv_image, text_for_image, (10, 25), font, 0.5, (0, 0, 0), 1, cv2.LINE_AA) cv2.imshow('test', cv_image) cv2.waitKey(1) except CvBridgeError as e: print(e) self.image = np.asarray(cv_image, dtype=np.uint8) self.transformed_image = self.do_transforms(self.image) # DO PREDICTION! if (self.got_odom and self.init): # setting up pose (x,y) input pose_input_np = np.asarray([self.x_pos, self.y_pos]) norm = np.linalg.norm(pose_input_np) if norm == 0: pose_input_norm = pose_input_np else: pose_input_norm = pose_input_np / norm pose_input = Variable((torch.from_numpy(pose_input_norm))) # setting up image input img_input = Variable(self.transformed_image) img_input = img_input.reshape((1, 1, 64, 64)) pose_input = pose_input.reshape((1, 1, 2)) if next(self.model.parameters()).is_cuda == True: img_input = img_input.to(torch.device("cuda")) pose_input = pose_input.to(torch.device("cuda")) output = self.model(pose_input, img_input) # _, pred = torch.max(output,1) # # self.dnn_classification = np.asarray(pred)[0] if next(self.model.parameters()).is_cuda == True: output_cpu = output.to(torch.device("cpu")) action = output_cpu.data.numpy() else: action = output.data.numpy() action = action.squeeze() self.rollrate = action[0] self.pitchrate = action[1] self.yawrate = action[2] self.thrust = action[3] self.dnn_output = RollPitchYawrateThrust() self.dnn_output.roll = self.rollrate self.dnn_output.pitch = self.pitchrate self.dnn_output.yaw_rate = self.yawrate self.dnn_output.thrust.z = self.thrust self.dnn_publisher.publish(self.dnn_output)
import sys import copy import rospy from mav_msgs.msg import RollPitchYawrateThrust from mavros_msgs.msg import AttitudeTarget from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import Quaternion import tf_conversions from armf import armtakeoff rospy.init_node('voxboxTOmavros',anonymous=True) msg = RollPitchYawrateThrust() setpoint = PoseStamped() setpoint.pose.position.x = 10 setpoint.pose.position.y = 10 setpoint.pose.position.z = 20 setpoint.header.frame_id = 'map' setpoint.header.stamp= rospy.Time.now() def callback(data): global msg msg = data main1()
mean = np.load(mean_std_dir + 'mean.npy') std = np.load(mean_std_dir + 'std.npy') print('mean', mean) print('std', std) exit() target_pose = PoseStamped() def target_cb(data): global target_pose target_pose = data mpc_u = RollPitchYawrateThrust() def mpc_cb(data): global mpc_u mpc_u = data def quaternion_to_euler_angle(w, x, y, z): ysqr = y * y t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + ysqr) X = math.degrees(math.atan2(t0, t1)) t2 = +2.0 * (w * y - z * x)
# initalize and set all the variables pid_x = PIDController(kp=kp_x, ki=ki_x, kd=kd_x, max_windup=1e6, u_bounds=[0, umax], alpha=alpha) pid_y = PIDController(kp=kp_y, ki=ki_y, kd=kd_y, max_windup=1e6, u_bounds=[0, umax], alpha=alpha) pid = PIDController(kp=kp, ki=ki, kd=kd, max_windup=1e6, u_bounds=[0, umax], alpha=alpha) empuje_global = 15.3000000 empuje_angles = RollPitchYawrateThrust() rospy.loginfo("Iniciando el nodo") talker() #rospy.spin() except rospy.ROSInterruptException: pass
kd = 0.3 umax = 5.0 # max controller output, (N) alpha = 1 # derivative filter smoothing factor # Simulation parameters N = 400 # number of simulation points t0 = 0 # starting time, (sec) tf = 4 # end time, (sec) time = np.linspace(t0, tf, N) dt = time[1] - time[0] # delta t, (sec) #y = [data.pose.pose.position.z,data.twist.twist.linear.z] y = [0, 0] # Initialize array to store values soln = np.zeros((len(time), len(y))) j = 0 # Create instance of PID_Controller class and # initalize and set all the variables pid = PIDController(kp=kp, ki=ki, kd=kd, max_windup=1e6, u_bounds=[0, umax], alpha=alpha) empuje_global = 15.3000000 empuje = RollPitchYawrateThrust() rospy.loginfo("Iniciando el nodo") talker() #rospy.spin() except rospy.ROSInterruptException: pass
def odom_cb(data): global odom odom = data vrpn = TwistStamped() def vrpn_cb(data): global vrpn vrpn = data rpyth = RollPitchYawrateThrust() def rpyth_cb(data): global rpyth rpyth = data def quaternion_to_euler_angle(w, x, y, z): ysqr = y * y t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + ysqr) X = math.degrees(math.atan2(t0, t1)) t2 = +2.0 * (w * y - z * x)
def do_control(): odom_sub = rospy.Subscriber('/f_450/odometry', Odometry, odom_cb, queue_size=100) vrpn_sub = rospy.Subscriber('/f_450/vrpn_client_node/f_450/twist', TwistStamped, vrpn_cb, queue_size=100) state_sub = rospy.Subscriber('/f_450/state_machine/state_info', String, state_cb, queue_size=100) target_pose_sub = rospy.Subscriber('/f_450/command/current_reference', MultiDOFJointTrajectory, target_cb, queue_size=100) mpc_sub = rospy.Subscriber('/f_450/command/roll_pitch_yawrate_thrust', RollPitchYawrateThrust, mpc_cb, queue_size=100) control_pub = rospy.Publisher( "/f_450/mavros/setpoint_raw/roll_pitch_yawrate_thrust", RollPitchYawrateThrust, queue_size=100) rospy.init_node('controller', anonymous=True) rate = rospy.Rate(50.0) global target_flag_pub target = np.array([-1.6, -0.5, 0.5, 0., 0., 0., 0., 0.], ndmin=2) while not rospy.is_shutdown(): x_f = odom.pose.pose.position.x y_f = odom.pose.pose.position.y z_f = odom.pose.pose.position.z vx_f = odom.twist.twist.linear.x vy_f = odom.twist.twist.linear.y vz_f = odom.twist.twist.linear.z (roll, pitch, yaw) = quaternion_to_euler_angle( odom.pose.pose.orientation.w, odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) r_f = math.radians(roll) p_f = math.radians(pitch) yaw_f = math.radians(yaw) rs_f = float(vrpn.twist.angular.x) ps_f = float(vrpn.twist.angular.y) ys_f = float(vrpn.twist.angular.z) # target = np.array([target_pose.pose.position.x ,target_pose.pose.position.y, target_pose.pose.position.z, 0.,0.,0., 0.,0.,0., 0.,0.,0.],ndmin=2) # state = np.array([x_f,y_f,z_f, vx_f,vy_f,vz_f, r_f,p_f,yaw_f, rs_f,ps_f,ys_f],ndmin=2) if target_flag_pub == True: target = np.array([ target_pose.points[0].transforms[0].translation.x, target_pose.points[0].transforms[0].translation.y, target_pose.points[0].transforms[0].translation.z, 0., 0., 0., 0., 0. ], ndmin=2) target_flag_pub = False state = np.array([x_f, y_f, z_f, vx_f, vy_f, vz_f, r_f, p_f], ndmin=2) inputs = state - target inputs = (inputs - mean) / std print(str(state_machine.data)) if str(state_machine.data) == 'PositionHold': print('MPC: ') controls = np.ravel(get_actions(inputs)) rpyth_mpc = RollPitchYawrateThrust() # rpyth_mpc.header.stamp = rospy.Time.now() # rpyth_mpc.roll = controls[0] # rpyth_mpc.pitch = controls[1] # rpyth_mpc.yaw_rate = mpc_u.yaw_rate # rpyth_mpc.thrust.z = (controls[2] * 7.5) + 7.5 rpyth_mpc.header.stamp = rospy.Time.now() rpyth_mpc.roll = mpc_u.roll rpyth_mpc.pitch = mpc_u.pitch rpyth_mpc.yaw_rate = mpc_u.yaw_rate rpyth_mpc.thrust.z = mpc_u.thrust.z control_pub.publish(rpyth_mpc) elif str(state_machine.data) == 'RcTeleOp' or str( state_machine.data) == 'RemoteControl' or str( state_machine.data) == 'HaveOdometry': print('MPC') rpyth_mpc = RollPitchYawrateThrust() rpyth_mpc.header.stamp = rospy.Time.now() rpyth_mpc.roll = mpc_u.roll rpyth_mpc.pitch = mpc_u.pitch rpyth_mpc.yaw_rate = mpc_u.yaw_rate rpyth_mpc.thrust.z = mpc_u.thrust.z control_pub.publish(rpyth_mpc) rate.sleep()
t1 = +1.0 - 2.0 * (x * x + ysqr) X = math.degrees(math.atan2(t0, t1)) t2 = +2.0 * (w * y - z * x) t2 = +1.0 if t2 > +1.0 else t2 t2 = -1.0 if t2 < -1.0 else t2 Y = math.degrees(math.asin(t2)) t3 = +2.0 * (w * z + x * y) t4 = +1.0 - 2.0 * (ysqr + z * z) Z = math.degrees(math.atan2(t3, t4)) return X, Y, Z actuators = RollPitchYawrateThrust() def control_cb(data): global actuators actuators = data trajectory = MarkerArray() def trajectory_cb(data): global trajectory trajectory = data
def do_control(): odom_sub = rospy.Subscriber(model + '/ground_truth/odometry', Odometry, odom_cb, queue_size=100) mpc_sub = rospy.Subscriber(model + '/rpyth', RollPitchYawrateThrust, mpc_cb, queue_size=100) target_pose_sub = rospy.Subscriber(model + '/command/pose', PoseStamped, target_cb, queue_size=100) motor_speed_pub = rospy.Publisher(model + "/command/roll_pitch_yawrate_thrust", RollPitchYawrateThrust, queue_size=100) rospy.init_node('controller', anonymous=True) rate = rospy.Rate(50.0) while not rospy.is_shutdown(): x_f = odom.pose.pose.position.x y_f = odom.pose.pose.position.y z_f = odom.pose.pose.position.z vx_f = odom.twist.twist.linear.x vy_f = odom.twist.twist.linear.y vz_f = odom.twist.twist.linear.z (roll, pitch, yaw) = quaternion_to_euler_angle( odom.pose.pose.orientation.w, odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) r_f = math.radians(roll) p_f = math.radians(pitch) yaw_f = math.radians(yaw) rs_f = (float(odom.twist.twist.angular.x)) ps_f = (float(odom.twist.twist.angular.y)) ys_f = (float(odom.twist.twist.angular.z)) target = np.array([ target_pose.pose.position.x, target_pose.pose.position.y, target_pose.pose.position.z, 0., 0., 0., 0., 0., 0., 0. ], ndmin=2) # target = np.array([1.,1.,1., 0.,0.,0., 0.,0., 0.,0.],ndmin=2) state = np.array( [x_f, y_f, z_f, vx_f, vy_f, vz_f, r_f, p_f, rs_f, ps_f], ndmin=2) inputs = state - target # Xtr = [[0.14604905, 0.51560138, 0.27780092, 0.8901413 , 0.90359776, 0.20449533, 0.27998261, 0.48336262, 0.36420253, 0.37711515]] # Xtr = np.reshape(Xtr, (1,10)) inputs = (inputs - mean) / std motor_controls = np.ravel(get_actions(inputs)) actuator = RollPitchYawrateThrust() actuator.header.stamp = rospy.Time.now() actuator.roll = motor_controls[0] actuator.pitch = motor_controls[1] # actuator.yaw_rate = motor_controls[2] actuator.thrust.z = (motor_controls[3] * 10.34) + (10.34) # actuator.roll = mpc_u.roll # actuator.pitch = mpc_u.pitch actuator.yaw_rate = mpc_u.yaw_rate # actuator.thrust.z = mpc_u.thrust.z print('roll: {:.5f}, pitch: {}, yaw_rate: {}, thrust: {}'.format( actuator.roll, actuator.pitch, actuator.yaw_rate, actuator.thrust.z)) motor_speed_pub.publish(actuator) rate.sleep()
def do_control( ): odom_sub = rospy.Subscriber('/'+model+'/ground_truth/odometry', Odometry, odom_cb, queue_size=100) target_pose_sub = rospy.Subscriber('/'+ model + '/command/current_reference', MultiDOFJointTrajectory ,target_cb, queue_size=100) mpc_sub = rospy.Subscriber('/' + model + '/rpyth', RollPitchYawrateThrust,mpc_cb, queue_size=100) control_pub = rospy.Publisher(model + "/command/roll_pitch_yawrate_thrust", RollPitchYawrateThrust,queue_size=100) rospy.init_node('controller',anonymous=True) rate = rospy.Rate(50.0) global target_flag_pub target = np.array([-1.6, -0.3,0.5, 0.,0.,0., 0.,0.],ndmin=2) while not rospy.is_shutdown(): x_f = odom.pose.pose.position.x y_f = odom.pose.pose.position.y z_f = odom.pose.pose.position.z vx_f = odom.twist.twist.linear.x vy_f = odom.twist.twist.linear.y vz_f = odom.twist.twist.linear.z (roll,pitch, yaw) = quaternion_to_euler_angle(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x , odom.pose.pose.orientation.y, odom.pose.pose.orientation.z) r_f = math.radians(roll) p_f = math.radians(pitch) yaw_f = math.radians(yaw) rs_f = (float(odom.twist.twist.angular.x)) ps_f = (float(odom.twist.twist.angular.y)) ys_f = (float(odom.twist.twist.angular.z)) # target = np.array([target_pose.pose.position.x ,target_pose.pose.position.y, target_pose.pose.position.z, 0.,0.,0., 0.,0.,0., 0.,0.,0.],ndmin=2) # state = np.array([x_f,y_f,z_f, vx_f,vy_f,vz_f, r_f,p_f,yaw_f, rs_f,ps_f,ys_f],ndmin=2) if target_flag_pub == True: target = np.array([target_pose.points[0].transforms[0].translation.x , target_pose.points[0].transforms[0].translation.y,target_pose.points[0].transforms[0].translation.z, 0.,0.,0., 0.,0., 0.,0.],ndmin=2) # print(target_flag_pub) target_flag_pub = False state = np.array([x_f,y_f,z_f, vx_f,vy_f,vz_f, r_f,p_f, rs_f,ps_f],ndmin=2) inputs = state - target inputs = (inputs-mean)/std controls = np.ravel(get_actions(inputs)) rpyth_mpc = RollPitchYawrateThrust() rpyth_mpc.header.stamp = rospy.Time.now() if(controller_type == 'dnn'): rpyth_mpc.roll = controls[0] rpyth_mpc.pitch = controls[1] rpyth_mpc.yaw_rate = mpc_u.yaw_rate rpyth_mpc.thrust.z = (controls[3] * constant_thrust) + constant_thrust print('DNN: roll: {:.3f} pitch: {:.3f} thrust: {:3f}'.format(rpyth_mpc.roll, rpyth_mpc.pitch, rpyth_mpc.thrust.z)) else: rpyth_mpc.roll = mpc_u.roll rpyth_mpc.pitch = mpc_u.pitch rpyth_mpc.yaw_rate = mpc_u.yaw_rate rpyth_mpc.thrust.z = mpc_u.thrust.z print('MPC: roll: {:.3f} pitch: {:.3f} thrust: {:3f}'.format(rpyth_mpc.roll, rpyth_mpc.pitch, rpyth_mpc.thrust.z)) print('Thrust Error: {:.3f}'.format(abs(z_f - target_pose.points[0].transforms[0].translation.z))) control_pub.publish(rpyth_mpc) rate.sleep()