def set_ang_vel_thrust(self, ang_vel, thrust, freq): """ Offboard method that sends angular velocity and thrust references to the PX4 autopilot of the the drone. Makes convertions from the local NED frame adopted for the ISR Flying Arena to the local ENU frame used by ROS. Converts the thrust from newtons to a normalized value between 0 and 1 through the mathematical expression of the thrust curve of the vehicle. Parameters ---------- ang_vel : np.array of floats with shape (3,1) Desired angular velocity for the drone. thrust : float Desired thrust value in newtons. freq : float Topic publishing frequency, in Hz. """ pub = rospy.Publisher(self.info.drone_ns + '/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) msg = AttitudeTarget() msg.header = Header() msg.header.stamp = rospy.Time.now() msg.type_mask = 128 msg.body_rate.x, msg.body_rate.y, msg.body_rate.z = ned_to_enu(ang_vel) msg.thrust = normalize_thrust(thrust, self.info.thrust_curve, norm(self.ekf.vel)) pub.publish(msg) rate = rospy.Rate(freq) rate.sleep()
def set_att_thrust(self, att, att_type, thrust, freq): """ Offboard method that sends attitude and thrust references to the PX4 autopilot of the the vehicle. Makes convertions from the local NED frame adopted for the ISR Flying Arena to the local ENU frame used by ROS. Converts the thrust from newtons to a normalized value between 0 and 1 through mathematical expression of the thrust curve of the vehicle. Parameters ---------- att : np.array of floats with shape (3,1) or with shape (4,1) Desired attitude for the vehicle, expressed in euler angles or in a quaternion. att_type : str Must be equal to either 'euler' or 'quaternion'. Specifies the format of the desired attitude. thrust : float Desired thrust value in newtons. freq : float Topic publishing frequency, in Hz. """ pub = rospy.Publisher(self.info.drone_ns + '/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) msg = AttitudeTarget() msg.header = Header() msg.header.stamp = rospy.Time.now() if att_type == 'euler': msg.orientation = Quaternion(*quaternion_from_euler( *ned_to_enu(att))) elif att_type == 'quaternion': msg.orientation = Quaternion(*SI_quaternion_to_ROS_quaternion(att)) msg.thrust = normalize_thrust(thrust, self.info.thrust_curve, norm(self.ekf.vel)) pub.publish(msg) rate = rospy.Rate(freq) rate.sleep()
def att_controller(self, r=0, p=0, y=0, z=2): if hasattr(self, "roll_zero"): r += self.roll_zero if hasattr(self, "pitch_zero"): p + self.pitch_zero att = AttitudeTarget() q = tf.transformations.quaternion_from_euler(r, p, y) att.orientation.x = q[0] att.orientation.y = q[1] att.orientation.z = q[2] att.orientation.w = q[3] rx, ry, rz, r, p, y = self.parse_local_position(mode="e") vx, vy, vz, wx, wy, wz = self.parse_velocity() try: param = 1.0 / (np.cos(r) * np.cos(p)) except Exception as e: param = 1 finally: vzt = self.pidz.step(z - rz) att.thrust = (self.pidzv.step(vzt - vz) + 0.57) * param # for plot only.. self.thrust = att.thrust self.pidzv_out = self.pidzv.out self.pidz_out = self.pidz.out att.header = Header() att.header.frame_id = "map" att.header.stamp = rospy.Time.now() self.att_setpoint_pub.publish(att)
def main1(): global setpoint # thrust_pub = rospy.Publisher('/mavros/setpoint_attitude/thrust',Thrust,queue_size=10) # thrust = Thrust() attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude',AttitudeTarget,queue_size=10) point_pub = rospy.Publisher('/waypoint',PoseStamped,queue_size=10) attitude = AttitudeTarget() attitude.type_mask = 3 attitude.header = msg.header # attitude.header.frame_id = 'FRAME_LOCAL_NED' # quaternion = tf.transformations.quaternion_from_euler(msg.roll,msg.pitch, 0) # attitude.orientation.x = quaternion[0] # attitude.orientation.y = quaternion[1] # attitude.orientation.z = quaternion[2] # attitude.orientation.w = quaternion[3] attitude.orientation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(msg.roll, msg.pitch, 0)) attitude.body_rate.z = msg.yaw_rate t = msg.thrust.z/100 if t>1: t=1 elif t<-1: t=-1 attitude.thrust = (t+1)/2 attitude_pub.publish(attitude) point_pub.publish(setpoint)
def track_and_trajectory_dock_control(self,des_trajectory_point, curr_odom, yaw_des, time_now, docker): if docker == self.num: pass else: return Rot_des = np.matrix(np.zeros((4, 4))) Rot_des[3, 3] = 1 attitude_des = AttitudeTarget() thrust = Thrust() des_acc = Vector3() if self.A_init_flag: thrust.header = curr_odom.header attitude_des.header = curr_odom.header dt = time_now - self.time_prev ex = des_trajectory_point.position.x - curr_odom.position.x ey = des_trajectory_point.position.y - curr_odom.position.y ez = des_trajectory_point.position.z - curr_odom.position.z evx = des_trajectory_point.velocity.x - curr_odom.velocity.x evy = des_trajectory_point.velocity.y - curr_odom.velocity.y evz = des_trajectory_point.velocity.z - curr_odom.velocity.z self.intx = self.intx + ex * dt self.inty = self.inty + ey * dt self.intz = self.intz + ez * dt des_acc.x = des_trajectory_point.acceleration.x + self.Kp_track.x * ex + self.Kd_track.x * evx + self.Ki_track.x * self.intx des_acc.y = des_trajectory_point.acceleration.y + self.Kp_track.y * ey + self.Kd_track.y * evy + self.Ki_track.y * self.inty des_acc.z = des_trajectory_point.acceleration.z + self.Kp_track.z * ez + self.Kd_track.z * evz + self.Ki_track.z * self.intz R_waitmod_w = np.matrix([[np.cos(self.tag_angle),-np.sin(self.tag_angle),0],[np.sin(self.tag_angle),np.cos(self.tag_angle),0],[0,0,1]]).transpose() curr_quaternion = [curr_odom.orientation.x, curr_odom.orientation.y, curr_odom.orientation.z, curr_odom.orientation.w] H_curr = tf.transformations.quaternion_matrix(curr_quaternion) Rot_curr = np.matrix(H_curr[:3, :3]) des_acc_relative = R_waitmod_w*np.matrix([[des_acc.x], [des_acc.y], [des_acc.z]]) Force_des = np.matrix([[0], [0], [self.m * self.g]])+self.m * des_acc_relative Force_des_body = Rot_curr * Force_des thrust.thrust = Force_des_body[2] Rot_des[:3, 2] = np.matrix(Force_des / LA.norm(Force_des)) x_body_in_world = np.matrix([[np.cos(des_trajectory_point.yaw)], [np.sin(des_trajectory_point.yaw)], [0]]) y_body_in_world = np.cross(Rot_des[:3, 2], x_body_in_world, axis=0) Rot_des[:3, 1] = np.matrix(y_body_in_world / LA.norm(y_body_in_world)) Rot_des[:3, 0] = np.matrix(np.cross(Rot_des[:3, 1], Rot_des[:3, 2], axis=0)) quad_des = tf.transformations.quaternion_from_matrix(Rot_des) attitude_des.orientation.x = quad_des[0] attitude_des.orientation.y = quad_des[1] attitude_des.orientation.z = quad_des[2] attitude_des.orientation.w = quad_des[3] attitude_des.thrust = thrust.thrust self.mavros_attitude_pub.publish(attitude_des) self.mavros_thrust_pub.publish(thrust) self.time_prev = time_now
def pos_control(self,des_trajectory_point,curr_pose, curr_vel, yaw_des, time_now): Rot_des = np.matrix(np.zeros((4, 4))) Rot_des[3, 3] = 1 attitude_des = AttitudeTarget() thrust = Thrust() des_acc = Vector3() if self.A_init_flag: thrust.header = curr_pose.header attitude_des.header = curr_pose.header dt = time_now - self.time_prev ex = des_trajectory_point.position.x - curr_pose.pose.position.x ey = des_trajectory_point.position.y - curr_pose.pose.position.y ez = des_trajectory_point.position.z - curr_pose.pose.position.z evx = des_trajectory_point.velocity.x - curr_vel.twist.linear.x evy = des_trajectory_point.velocity.y - curr_vel.twist.linear.y evz = des_trajectory_point.velocity.z - curr_vel.twist.linear.z self.intx = self.intx + ex * dt self.inty = self.inty + ey * dt self.intz = self.intz + ez * dt des_acc.x = des_trajectory_point.acceleration.x + self.Kp.x * ex + self.Kd.x * evx + self.ki.x * self.intx des_acc.y = des_trajectory_point.acceleration.y + self.Kp.y * ey + self.Kd.y * evy + self.ki.y * self.inty des_acc.z = des_trajectory_point.acceleration.z + self.Kp.z * ez + self.Kd.z * evz + self.ki.z * self.intz curr_quaternion = [curr_pose.pose.orientation.x, curr_pose.pose.orientation.y, curr_pose.pose.orientation.z, curr_pose.pose.orientation.w] H_curr = tf.transformations.quaternion_matrix(curr_quaternion) Rot_curr = np.matrix(H_curr[:3, :3]) Force_des = np.matrix([[0], [0], [self.m * self.g]])+self.m * np.matrix([[des_acc.x], [des_acc.y], [des_acc.z]]) Force_des_body = Rot_curr * Force_des thrust.thrust = Force_des_body[2] Rot_des[:3, 2] = np.matrix(Force_des / LA.norm(Force_des)) x_body_in_world = np.matrix([[np.cos(des_trajectory_point.yaw)], [np.sin(des_trajectory_point.yaw)], [0]]) y_body_in_world = np.cross(Rot_des[:3, 2], x_body_in_world, axis=0) Rot_des[:3, 1] = np.matrix(y_body_in_world / LA.norm(y_body_in_world)) Rot_des[:3, 0] = np.matrix(np.cross(Rot_des[:3, 1], Rot_des[:3, 2], axis=0)) quad_des = tf.transformations.quaternion_from_matrix(Rot_des) attitude_des.orientation.x = quad_des[0] attitude_des.orientation.y = quad_des[1] attitude_des.orientation.z = quad_des[2] attitude_des.orientation.w = quad_des[3] attitude_des.thrust = thrust.thrust self.mavros_attitude_pub.publish(attitude_des) self.mavros_thrust_pub.publish(thrust) self.time_prev = time_now
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()