def main(): # INITIALIZE ADAPTIVE NETWORK PARAMETERS: N_INPUT = 3 # Number of input network N_OUTPUT = 1 # Number of output network INIT_NODE = 1 # Number of node(s) for initial structure INIT_LAYER = 2 # Number of initial layer (minimum 2, for 1 hidden and 1 output) N_WINDOW = 500 # Sliding Window Size EVAL_WINDOW = 3 # Evaluation Window for layer Adaptation DELTA = 0.05 # Confidence Level for layer Adaptation (0.05 = 95%) ETA = 0.0001 # Minimum allowable value if divided by zero FORGET_FACTOR = 0.98 # Forgetting Factor of Recursive Calculation #EXP_DECAY = 1 # Learning Rate decay factor #LEARNING_RATE = 0.00005 # Network optimization learning rate LEARNING_RATE = 0.00002 / 2 # Network optimization learning rate WEIGHT_DECAY = 0.000125 / 5 # Regularization weight decay factor #WEIGHT_DECAY = 0.0 # Regularization weight decay factor # INITIALIZE SYSTEM AND SIMULATION PARAMETERS: IRIS_THRUST = 0.5629 # Nominal thrust for IRIS Quadrotor RATE = 30.0 # Sampling Frequency (Hz) # PID_GAIN_X = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-X # PID_GAIN_Y = [0.25,0.0,0.02] # PID Gain Parameter [KP,KI,KD] axis-Y PID_GAIN_X = [0.15, 0.0, 0.004] # PID Gain Parameter [KP,KI,KD] axis-X PID_GAIN_Y = [0.15, 0.0, 0.004] # PID Gain Parameter [KP,KI,KD] axis-Y #PID_GAIN_Z = [0.013,0.0004,0.2] # PID Gain Parameter [KP,KI,KD] axis-Z PID_GAIN_Z = [0.013, 0.0, 0.2] # PID Gain Parameter [KP,KI,KD] axis-Z SIM_TIME = 200 # Simulation time duration (s) # Initial conditions of UAV system xref = 0.0 yref = 0.0 zref = 2.0 interrx = 0.0 interry = 0.0 interrz = 0.0 errlastx = 0.0 errlasty = 0.0 errlastz = 0.0 runtime = 0.0 # Ignite the Evolving NN Controller Xcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE) Ycon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE) Zcon = NetEvo(N_INPUT, N_OUTPUT, INIT_NODE) if INIT_LAYER > 2: pdb.set_trace() for i in range(INIT_LAYER - 2): Xcon.addhiddenlayer() Ycon.addhiddenlayer() Zcon.addhiddenlayer() dt = 1 / RATE Xcon.dt = dt Ycon.dt = dt Zcon.dt = dt Xcon.smcpar = PID_GAIN_X Ycon.smcpar = PID_GAIN_Y Zcon.smcpar = PID_GAIN_Z # PX4 API object modes = fcuModes() # Flight mode object uav = uavCommand() # UAV command object # Data Logger object logData = dataLogger() xconLog = dataLogger() yconLog = dataLogger() zconLog = dataLogger() # Initiate node and subscriber rospy.init_node('setpoint_node', anonymous=True) rate = rospy.Rate(RATE) # ROS loop rate, [Hz] # Subscribe to drone state rospy.Subscriber('mavros/state', State, uav.stateCb) # Subscribe to drone's local position #rospy.Subscriber('mavros/local_position/pose', PoseStamped, uav.posCb) rospy.Subscriber('mavros/mocap/pose', PoseStamped, uav.posCb) # Setpoint publisher att_sp_pub = rospy.Publisher('mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10) # Initiate Attitude messages att = AttitudeTarget() att.body_rate = Vector3() att.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) att.thrust = IRIS_THRUST att.type_mask = 7 # Arming the UAV --> no auto arming text = colored("Ready for Arming..Press Enter to continue...", 'green', attrs=['reverse', 'blink']) raw_input(text) while not uav.state.armed: modes.setArm() rate.sleep() text = colored('Vehicle is arming..', 'yellow') print(text) # Switch to OFFBOARD after send few setpoint messages text = colored("Vehicle Armed!! Press Enter to switch OFFBOARD...", 'green', attrs=['reverse', 'blink']) raw_input(text) while not uav.state.armed: modes.setArm() rate.sleep() text = colored('Vehicle is arming..', 'yellow') print(text) rate.sleep() k = 0 while k < 10: att_sp_pub.publish(att) rate.sleep() k = k + 1 modes.setOffboardMode() text = colored('Now in OFFBOARD Mode..', 'blue', attrs=['reverse', 'blink']) print(text) # ROS Main loop:: k = 0 while not rospy.is_shutdown(): start = time.time() rate.sleep() # Setpoint generator if runtime <= 20: xref = 0.0 yref = 0.0 zref = runtime * 1.5 / 20.0 if runtime > 20 and runtime <= 30: xref = (runtime - 20) * 1.0 / 10.0 #xref = (runtime-20)* 5.0/10.0 yref = 0.0 zref = 1.5 if runtime > 30: xref = 0.0 + 1.0 * math.cos(math.pi * (runtime - 30) / 20.0) yref = 0.0 + 1.0 * math.sin(math.pi * (runtime - 30) / 20.0) #xref = 0.0+5.0*math.cos(math.pi*(runtime-30)/60.0) #yref = 0.0+5.0*math.sin(math.pi*(runtime-30)/60.0) zref = 1.5 # Adjust the number of nodes in the latest layer (Network Width) Xcon.adjustWidth(FORGET_FACTOR, N_WINDOW) Ycon.adjustWidth(FORGET_FACTOR, N_WINDOW) #Zcon.adjustWidth(FORGET_FACTOR,N_WINDOW) # Adjust the number of layer (Network Depth) Xcon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW) Ycon.adjustDepth(ETA, DELTA, N_WINDOW, EVAL_WINDOW) #Zcon.adjustDepth(ETA,DELTA,N_WINDOW,EVAL_WINDOW) # update current position xpos = uav.local_pos.x ypos = uav.local_pos.y zpos = uav.local_pos.z # calculate errors,delta errors, and integral errors errx, derrx, interrx = Xcon.calculateError(xref, xpos) erry, derry, interry = Ycon.calculateError(yref, ypos) errz, derrz, interrz = Zcon.calculateError(zref, zpos) # PID Controller equations #theta_ref = 0.04*errx+0.0005*interrx+0.01*derrx #phi_ref = 0.04*erry+0.0005*interry+0.01*derry # vx = PID_GAIN_X[0]*errx+PID_GAIN_X[1]*interrx+PID_GAIN_X[2]*derrx # vy = PID_GAIN_Y[0]*erry+PID_GAIN_Y[1]*interry+PID_GAIN_Y[2]*derry #thrust_ref = IRIS_THRUST+PID_GAIN_Z[0]*errz+PID_GAIN_Z[1]*interrz+PID_GAIN_Z[2]*derrz # SMC + NN controller vx = Xcon.controlUpdate(xref) # Velocity X vy = Ycon.controlUpdate(yref) # Velocity Y thz = Zcon.controlUpdate(zref) # Additional Thrust Z euler = euler_from_quaternion(uav.q) psi = euler[2] phi_ref, theta_ref = transform_control_signal(vx, vy, psi) thrust_ref = IRIS_THRUST + thz # control signal limiter #phi_ref = limiter(-1*phi_ref,0.2) phi_ref = limiter(-1 * phi_ref, 0.25) theta_ref = limiter(-1 * theta_ref, 0.25) att.thrust = limiter(thrust_ref, 1) # Phi, Theta and Psi quaternion transformation att.orientation = Quaternion( *quaternion_from_euler(phi_ref, theta_ref, 0.0)) # Publish the control signal att_sp_pub.publish(att) # NN Learning stage Xcon.optimize(LEARNING_RATE, WEIGHT_DECAY) Ycon.optimize(LEARNING_RATE, WEIGHT_DECAY) Zcon.optimize(LEARNING_RATE, WEIGHT_DECAY) # Print all states print('Xr,Yr,Zr = ', xref, yref, zref) print('X, Y, Z = ', uav.local_pos.x, uav.local_pos.y, uav.local_pos.z) print('phi, theta = ', phi_ref, theta_ref) print('att angles = ', euler) print('Thrust = ', att.thrust) print('Nodes X Y Z = ', Xcon.nNodes, Ycon.nNodes, Zcon.nNodes) print('Layer X Y Z = ', Xcon.nLayer, Ycon.nLayer, Zcon.nLayer) k += 1 runtime = k * dt # Append Data Logger logData.appendStateData(runtime, xref, yref, zref, xpos, ypos, zpos, phi_ref, theta_ref, thrust_ref) xconLog.appendControlData(runtime, Xcon.us, Xcon.un, Xcon.u, Xcon.nNodes) yconLog.appendControlData(runtime, Ycon.us, Ycon.un, Ycon.u, Ycon.nNodes) zconLog.appendControlData(runtime, Zcon.us, Zcon.un, Zcon.u, Zcon.nNodes) print('Runtime = ', runtime) print('Exec time = ', time.time() - start) print('') # Break condition if runtime > SIM_TIME: modes.setRTLMode() text = colored('Auto landing mode now..', 'blue', attrs=['reverse', 'blink']) print(text) text = colored('Now saving all logged data..', 'yellow', attrs=['reverse', 'blink']) print(text) logData.plotFigure() xconLog.plotControlData() yconLog.plotControlData() zconLog.plotControlData() text = colored('Mission Complete!!', 'green', attrs=['reverse', 'blink']) print(text) break
def main(args): global list_current_position, list_current_velocity, current_position, usingvelocitycontrol, usingpositioncontrol, xvel, yvel, state, cur_pose, list_error, target global global_obs_detected, list_velocity_angle, vController par.CurrentIteration = 1 if (len(args) > 1): uav_ID = str(args[1]) else: uav_ID = "1" idx = int(uav_ID) - 1 rospy.init_node("control_uav_" + uav_ID) print "UAV" + uav_ID vController = VelocityController() try: rospy.wait_for_service("/uav" + uav_ID + "/mavros/cmd/arming") rospy.wait_for_service("/uav" + uav_ID + "/mavros/set_mode") except rospy.ROSException: fail("failed to connect to service") state_sub = rospy.Subscriber("/uav" + uav_ID + "/mavros/state", State, queue_size=10, callback=state_cb) local_vel_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10) local_pos_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_position/local", PoseStamped, queue_size=10) local_pos_target = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_raw/local", PositionTarget, queue_size=10) atittude_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=10) thr_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_attitude/att_throttle", Float64, queue_size=10) Astar_grid_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_grid", GridCells, queue_size=10) Astar_path_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_path", GridCells, queue_size=10) arming_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/cmd/arming", CommandBool) set_mode_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/set_mode", SetMode) for i in range(4): #velocity_sub = rospy.Subscriber("/uav"+repr(i+1)+"/mavros/local_position/velocity",TwistStamped,queue_size = 10,callback=velocity_cb, callback_args=i) position_sub = rospy.Subscriber("/uav" + repr(i + 1) + "/mavros/local_position/pose", PoseStamped, queue_size=10, callback=position_cb, callback_args=(i, int(uav_ID) - 1)) scan_lidar_sub = rospy.Subscriber("/scan", LaserScan, queue_size=10, callback=scan_lidar_cb, callback_args=uav_ID) br = tf.TransformBroadcaster() r = rospy.Rate(10) print "TRY TO CONNECT" while ((not rospy.is_shutdown()) and (not current_state.connected)): #rospy.spinOnce() r.sleep() #print(current_state.connected.__class__) rospy.loginfo("CURRENT STATE CONNECTED") poses = PoseStamped() #poses.pose = Pose() #poses.pose.position = Point() target = Pose() if (idx == 0): target.position.x = 0 target.position.y = 0 if (idx == 1): target.position.x = 0 target.position.y = 0 if (idx == 2): target.position.x = 0 target.position.y = 0 if (idx == 3): target.position.x = 4 target.position.y = 4 target.position.z = 10 poses.pose.position.x = target.position.x poses.pose.position.y = target.position.y poses.pose.position.z = target.position.z q = quaternion_from_euler(0, 0, 45 * np.pi / 180.0) poses.pose.orientation.x = q[0] poses.pose.orientation.y = q[1] poses.pose.orientation.z = q[2] poses.pose.orientation.w = q[3] i = 100 #while((not rospy.is_shutdown()) and (i>0)): # local_pos_pub.publish(poses) # i = i-1 rviz_visualization_start = False last_request = rospy.Time.now() count = 0 if (idx == 1): target.position.x = 28 target.position.y = 28 if (idx == 2): target.position.x = 0 target.position.y = 17 #thread1 = Thread(target = key_function) #thread1.start() style.use('fivethirtyeight') thread2 = Thread(target=plot_error_live) thread2.start() thread3 = Thread(target=calculate_and_publish_wall, args=(Astar_grid_pub, Astar_path_pub, idx)) thread3.start() #file_error_pos_x = open(dir_path+'/txt/error_pos_x.txt','w') #file_error_pos_y = open(dir_path+'/txt/error_pos_y.txt','w') #file_error_pos_z = open(dir_path+'/txt/error_pos_z.txt','w') error_time = 0 print poses uav_color = [255, 0, 0, 255] topic = 'uav_marker_' + uav_ID uav_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10) uav_marker = Marker() uav_marker.header.frame_id = "world" uav_marker.type = uav_marker.SPHERE uav_marker.action = uav_marker.ADD uav_marker.scale.x = par.ws_model['robot_radius'] * 3 uav_marker.scale.y = par.ws_model['robot_radius'] * 3 uav_marker.scale.z = 0.005 * par.RealScale uav_marker.color.r = float(uav_color[0]) / 255 uav_marker.color.g = float(uav_color[1]) / 255 uav_marker.color.b = float(uav_color[2]) / 255 uav_marker.color.a = float(uav_color[3]) / 255 uav_marker.pose.orientation.w = 1.0 uav_marker.pose.position.x = 0 #init_pos[0] uav_marker.pose.position.y = 0 #init_pos[1] uav_marker.pose.position.z = 0 #init_pos[2] uav_marker_publisher.publish(uav_marker) uav_color = [255, 255, 255, 255] topic = 'uav_goal_marker_' + uav_ID uav_goal_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10) uav_goal_marker = Marker() uav_goal_marker.header.frame_id = "world" uav_goal_marker.type = uav_goal_marker.SPHERE uav_goal_marker.action = uav_goal_marker.ADD uav_goal_marker.scale.x = par.ws_model['robot_radius'] * 2 uav_goal_marker.scale.y = par.ws_model['robot_radius'] * 2 uav_goal_marker.scale.z = 0.005 * par.RealScale uav_goal_marker.color.r = float(uav_color[0]) / 255 uav_goal_marker.color.g = float(uav_color[1]) / 255 uav_goal_marker.color.b = float(uav_color[2]) / 255 uav_goal_marker.color.a = float(uav_color[3]) / 255 uav_goal_marker.pose.orientation.w = 1.0 uav_goal_marker.pose.position.x = 0 #init_pos[0] uav_goal_marker.pose.position.y = 0 #init_pos[1] uav_goal_marker.pose.position.z = 0 #init_pos[2] uav_goal_marker_publisher.publish(uav_goal_marker) uav_goal_marker = update_uav_marker( uav_goal_marker, (target.position.x, target.position.y, target.position.z, 1.0)) uav_goal_marker_publisher.publish(uav_goal_marker) last_request_rviz = rospy.Time.now() last_HRVO_request = rospy.Time.now() while (not rospy.is_shutdown()): if (rviz_visualization_start and (rospy.Time.now() - last_request_rviz > rospy.Duration(0.2))): publish_uav_position_rviz( br, list_current_position[idx].pose.position.x, list_current_position[idx].pose.position.y, list_current_position[idx].pose.position.z, uav_ID) uav_marker = update_uav_marker( uav_marker, (list_current_position[idx].pose.position.x, list_current_position[idx].pose.position.y, list_current_position[idx].pose.position.z, 1.0)) uav_marker_publisher.publish(uav_marker) last_request_rviz = rospy.Time.now() if ((not rviz_visualization_start) and (current_state.mode != 'OFFBOARD') and (rospy.Time.now() - last_request > rospy.Duration(1.0))): offb_set_mode = set_mode_client(0, 'OFFBOARD') if (offb_set_mode.mode_sent): rospy.loginfo("OFFBOARD ENABLED") last_request = rospy.Time.now() else: if ((not current_state.armed) and (rospy.Time.now() - last_request > rospy.Duration(1.0))): arm_cmd = arming_client(True) if (arm_cmd.success): rospy.loginfo("VEHICLE ARMED") rviz_visualization_start = True last_request = rospy.Time.now() #print distance3D(cur_pose.pose,target) if (distance3D(cur_pose.pose, target) < 0.5) and (not usingvelocitycontrol): print "sampai" usingvelocitycontrol = True usingpositioncontrol = False target = Pose() if (idx == 0): target.position.x = 28 target.position.y = 28 if (idx == 1): target.position.x = 0 target.position.y = 0 if (idx == 2): target.position.x = 21 target.position.y = 14 if (idx == 3): target.position.x = 0 target.position.y = 0 target.position.z = 10 print target psi_target = 45 * np.pi / 180.0 #np.pi/180.0 #np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x) diagram4 = GridWithWeights(par.NumberOfPoints, par.NumberOfPoints) diagram4.walls = [] for i in range(par.NumberOfPoints ): # berikan wall pada algoritma pathfinding A* for j in range(par.NumberOfPoints): if (par.ObstacleRegion[j, i] == 0): diagram4.walls.append((i, j)) for i in range(par.NumberOfPoints): for j in range(par.NumberOfPoints): if (par.ObstacleRegionWithClearence[j][i] == 0): diagram4.weights.update({(i, j): par.UniversalCost}) goal_pos = (target.position.x / par.RealScale, target.position.y / par.RealScale) start_pos = (cur_pose.pose.position.x / par.RealScale, cur_pose.pose.position.y / par.RealScale) pathx, pathy = Astar_version1(par, start_pos, goal_pos, diagram4) print pathx print pathy vController.setHeadingTarget(deg2rad(90.0)) #45.0)) target.position.x = pathx[0] * par.RealScale target.position.y = pathy[0] * par.RealScale uav_goal_marker = update_uav_marker( uav_goal_marker, (target.position.x, target.position.y, target.position.z, 1.0)) uav_goal_marker_publisher.publish(uav_goal_marker) vController.setTarget(target) vController.setPIDGainX(1, 0.0, 0.0) vController.setPIDGainY(1, 0.0, 0.0) vController.setPIDGainZ(1, 0, 0) vController.setPIDGainPHI(1, 0.0, 0.0) vController.setPIDGainTHETA(1, 0.0, 0.0) vController.setPIDGainPSI(1, 0.0, 0.0) if (usingpositioncontrol): print "kontrol posisi" poses.pose.position.x = 29 poses.pose.position.y = 29 poses.pose.position.z = 10 local_pos_pub.publish(poses) elif (usingvelocitycontrol): if (distance2D(cur_pose.pose, target) < 1.5): if (len(pathx) > 1): del pathx[0] del pathy[0] target.position.x = pathx[0] * par.RealScale target.position.y = pathy[0] * par.RealScale uav_goal_marker = update_uav_marker( uav_goal_marker, (target.position.x, target.position.y, target.position.z, 1.0)) uav_goal_marker_publisher.publish(uav_goal_marker) print target vController.setTarget(target) if (rospy.Time.now() - last_HRVO_request > rospy.Duration(0.05)): last_HRVO_request = rospy.Time.now() header.stamp = rospy.Time.now() des_vel = vController.update(cur_pose) current_agent_pose = ( list_current_position[idx].pose.position.x, list_current_position[idx].pose.position.y, list_current_position[idx].pose.position.z) current_agent_vel = (list_current_velocity[idx].twist.linear.x, list_current_velocity[idx].twist.linear.y) current_neighbor_pose = [] for i in range(par.NumberOfAgents): if (i != idx): current_neighbor_pose.append( (list_current_position[i].pose.position.x, list_current_position[i].pose.position.y)) current_neighbor_vel = [] for i in range(par.NumberOfAgents): if (i != idx): current_neighbor_vel.append( (list_current_velocity[i].twist.linear.x, list_current_velocity[i].twist.linear.y)) V_des = (des_vel.twist.linear.x, des_vel.twist.linear.y) quat_arr = np.array([ list_current_position[idx].pose.orientation.x, list_current_position[idx].pose.orientation.y, list_current_position[idx].pose.orientation.z, list_current_position[idx].pose.orientation.w ]) att = euler_from_quaternion(quat_arr, 'sxyz') #V = locplan.RVO_update_single(current_agent_pose, current_neighbor_pose, current_agent_vel, current_neighbor_vel, V_des, par,list_velocity_angle,list_distance_obs,att[2]) V, RVO_BA_all = locplan.RVO_update_single_static( current_agent_pose, current_neighbor_pose, current_agent_vel, current_neighbor_vel, V_des, par) V_msg = des_vel V_msg.twist.linear.x = V[0] V_msg.twist.linear.y = V[1] local_vel_pub.publish(V_msg) goal = (target.position.x, target.position.y, target.position.z) x = current_position list_error[0].append(error_time) list_error[1].append(goal[0] - x[0]) list_error[2].append(goal[1] - x[1]) list_error[3].append(goal[2] - x[2]) error_time = error_time + 1 k = 0.1 vx = k * (goal[0] - x[0]) vy = k * (goal[1] - x[1]) vz = k * (goal[2] - x[2]) postar = PositionTarget() postar.header = header postar.coordinate_frame = PositionTarget().FRAME_LOCAL_NED p = PositionTarget().IGNORE_PX | PositionTarget( ).IGNORE_PY | PositionTarget().IGNORE_PZ a = PositionTarget().IGNORE_AFX | PositionTarget( ).IGNORE_AFY | PositionTarget().IGNORE_AFZ v = PositionTarget().IGNORE_VX | PositionTarget( ).IGNORE_VY | PositionTarget().IGNORE_VZ y = PositionTarget().IGNORE_YAW | PositionTarget( ).IGNORE_YAW_RATE f = PositionTarget().FORCE postar.type_mask = a | y | p | f #| PositionTarget().IGNORE_YAW | PositionTarget().IGNORE_YAW_RATE | PositionTarget().FORCE postar.velocity.x = vx postar.velocity.y = vy postar.velocity.z = vz postar.position.x = goal[0] postar.position.y = goal[1] postar.position.z = goal[2] vel_msg = TwistStamped() vel_msg.header = header vel_msg.twist.linear.x = xvel vel_msg.twist.linear.y = yvel vel_msg.twist.linear.z = 0.0 vel_msg.twist.angular.x = 0.0 vel_msg.twist.angular.y = 0.0 vel_msg.twist.angular.z = 0.0 q = quaternion_from_euler(0, 0, 0.2) att = PoseStamped() att.pose.orientation.x = q[0] att.pose.orientation.y = q[1] att.pose.orientation.z = q[2] att.pose.orientation.w = q[3] att.pose.position.x = 0.0 att.pose.position.y = 0.0 att.pose.position.z = 2.0 cmd_thr = Float64() cmd_thr.data = 0.3 att_raw = AttitudeTarget() att_raw.header = Header() att_raw.body_rate = Vector3() att_raw.thrust = 0.7 #Float64() att_raw.header.stamp = rospy.Time.now() att_raw.header.frame_id = "fcu" att_raw.type_mask = 71 att_raw.orientation = Quaternion( *quaternion_from_euler(0, 0, 0.2)) else: local_pos_pub.publish(poses) count = count + 1 r.sleep() try: rospy.spin() except KeyboardInterrupt: print("Shutting down") cv2.destroyAllWindows()
from time import sleep import rospy from geometry_msgs.msg import Quaternion from geometry_msgs.msg import Vector3 from mavros_msgs.msg import Thrust from mavros_msgs.msg import AttitudeTarget node = rospy.init_node("thrust_test") rate = rospy.Rate(20) # Hz pub = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) print("looping!") while True: msg = AttitudeTarget() msg.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) msg.body_rate = Vector3(x=0.0, y=0.0, z=0.0) msg.thrust = -0.001 pub.publish(msg) rate.sleep() print("Sending thrust [%f]!" % msg.thrust)