def droneTakeoff(self, alt=1.0): if self.isAirbourne == False or self.sysState == 'idle': if not self.droneArmed: mavCMD.arming(True) preArmMsgs = self.curPos preArmMsgs.pose.position.z = alt for i in range(50): self._pubMsg(preArmMsgs, self.spLocalPub) self.setState('takeoff') self.setMode(0, 'OFFBOARD') self.setPoint = preArmMsgs self.isAirbourne = True self.messageHandlerPub.publish(self.isAirbourne) # wait until takeoff has occurred while (self.curPos.pose.position.z <= (preArmMsgs.pose.position.z - 0.25)): self._pubMsg(self.setPoint, self.spLocalPub) else: self.setState('landing') self.setMode(0, 'AUTO.LAND') while self.uavState.armed: self.rate.sleep() self.isAirbourne = False self.setState('idle')
def __init__(self): # Arm the drone mavros.set_namespace() command.arming(True) self.pub_sp = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) rospy.wait_for_service('mavros/set_mode') change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode) rospy.init_node('gotowaypoint', anonymous=True) rate = rospy.Rate(50) rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_cb) self.waypoint = PoseStamped() self.waypoint.header.frame_id = 'map' self.waypoint.pose.position.x = 0 self.waypoint.pose.position.y = 0 self.waypoint.pose.position.z = 5 self.marker_pub = rospy.Publisher('/waypoint_marker', Marker, queue_size=10) self.waypoint_marker = Marker() self.waypoints = np.array([[0,0,0], [0,1,1], [0,3,3]]) self.publish_waypoint() while not rospy.is_shutdown(): result_mode = change_mode(0,"OFFBOARD") self.pub_sp.publish(self.waypoint) rate.sleep()
def joy_cb(self, data): if self.state_mach == self.DISARMED and data.buttons[20] == 1: mavros.set_namespace() self.set_offboard_mode() elif self.state_mach == self.OFFBOARD and data.buttons[3] == 1: command.arming(True) elif self.state_mach == self.ARMED and data.buttons[4] == 1: command.arming(False)
def __init__(self): # Arm the drone mavros.set_namespace() command.arming(True) self.pub_sp = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10) rospy.init_node('gotowaypoint', anonymous=True) rate = rospy.Rate(60) # 10hz # Parse input #parser = argparse.ArgumentParser() #parser.add_argument("--boundary", help="filepath to the control boundary") #args = parser.parse_args(rospy.myargv(argsv)) #self.pub_sp = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) rospy.wait_for_service('mavros/set_mode') self.change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode) data = self.load_config('scripts/boundary.yaml') self.boundary = Boundary(data) self.controller = QUAD_position_controller( self.load_config('scripts/gainsStart.yaml')) # Set parameters self.kz = 0.05 self.hoverth = 0.565 # Subscribe to local position self.local_pose = PoseStamped() self.velocity = TwistStamped() rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._send_command) rospy.Subscriber('/mavros/local_position/velocity_local', TwistStamped, self._read_velocity) # Set Waypoint self.waypoint = PoseStamped() self.waypoint.pose.position.x = -1.4 self.waypoint.pose.position.y = -5 self.waypoint.pose.position.z = 3 self.marker_pub = rospy.Publisher('/waypoint_marker', Marker, queue_size=10) self.waypoint_marker = Marker() self.publish_waypoint() # Define Controller rospy.spin()
def joy_cb(self, data): if self.state_mach == self.DISARMED and data.buttons[20] == 1: mavros.set_namespace() self.set_offboard_mode() elif self.state_mach == self.OFFBOARD and data.buttons[3] == 1: command.arming(True) elif self.state_mach == self.ARMED and data.buttons[4] == 1: command.arming(False) elif self.state_switch_mode == self.UNSWITCHED and data.buttons[7] == 1: self.state_switch_mode = self.SWITCHED doS = rospy.ServiceProxy('doSwitch', CommandBool) doS(True) elif self.state_switch_mode == self.SWITCHED and data.buttons[8] == 1: self.state_switch_mode = self.UNSWITCHED doS = rospy.ServiceProxy('doSwitch', CommandBool) doS(False)
def __init__(self, argsv): # Init Node self.main_loop_rate = 60 self.is_simulation = True self.p = namedtuple("p", "x y z") self.q = namedtuple("q", "w x y z") self.v = namedtuple("v", "x y z") self.omg = namedtuple("omg", "x y z") self.p.x, self.p.y, self.p.z, self.q.w, self.q.x, self.q.y, self.q.z, self.v.x, self.v.y, self.v.z, self.omg.x,\ self.omg.y, self.omg.z = 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0. self.u_current = np.zeros((1, 4)) self.T_d = 0.0 self.q_d = namedtuple("q_d", "w x y z") self.omg_d = namedtuple("omg_d", "x y z") self.f_d = namedtuple( "f_d", "x y z") #Variable used to publish desired force commands self.init_ROS() stabilize_time = 1.0 state = 'not_launched' z_th = 0.5 # Armrq mavros.set_namespace() command.arming(True) while (not rospy.is_shutdown()): # Run State Machine if (state == 'not_launched'): if (self.p.z > z_th): launch_time = rospy.Time().now() state = 'launched' print('Launch Detected.') elif (state == 'launched'): if ((rospy.Time().now() - launch_time).to_sec() > stabilize_time): self.change_mode(0, "POSCTL") state = 'stabilize' print('Stabilize mode set.') # Check altitude self.rate.sleep()
def main(): rospy.init_node('px4_offboard_python', anonymous=True) #state_sub = rospy.Subscriber("mavros/state", ) #while not rospy.is_shutdown() and not current_state.connected: # rospy.Rate(20) #rospy.loginfo('FCU connection successful') #rospy.Subscriber("chatter", String, callback) pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) #local_pos_pub = get_pub_position_local(queue_size=10) #arming_client = rospy.ServiceProxy("/mavros/arming", CommandBool) set_mode = rospy.ServiceProxy("/mavros/set_mode", SetMode) rate = rospy.Rate(20) msg = PoseStamped() msg.header = Header() msg.header.stamp = rospy.Time.now() msg.pose.position.x = 0 msg.pose.position.y = 0 msg.pose.position.z = 0.5 #for i in range(100): # local_pos_pub.Publish(msg) # rate.sleep() #offb_set_mode = SetMode() #offb_set_mode.request.custom_mode('OFFBOARD') #rospy.loginfo("Drone offboard!") req = SetModeRequest() req.custom_mode = 'OFFBOARD' set_mode.call(req) print 'mode: ' + State().mode mavros.set_namespace() command.arming(True) rospy.loginfo("Drone armed!") rospy.spin()
def _arm(self, state): try: ret = command.arming(value=state) except rospy.ServiceException as ex: fault(ex) if not ret.success: fault("Request failed. Check mavros logs") return ret
def _arm(self, state): try: ret = command.arming(value=state) if not ret.success: rospy.logerr("Request failed. Check mavros logs") rospy.loginfo("Command result: %s" % str(ret.result)) except rospy.ServiceException as ex: rospy.logerr(ex)
def droneTakeoff(self, alt=1.0): if self.isAirbourne == False or self.sysState == 'idle': if not self.MAVROS_State.armed: mavCMD.arming(True) preArmMsgs = self.uavLocalPos preArmMsgs.pose.position.z = 1.5 for i in range(50): self._pubMsg(preArmMsgs, self.spLocalPub) self._setState('takeoff') self.setMode(0, 'OFFBOARD') self.isAirbourne = True self.enableMHPub.publish(self.isAirbourne) #wait until takeoff has occurred while (self.uavLocalPos.pose.position.z <= (preArmMsgs.pose.position.z - 0.25)): self._pubMsg(preArmMsgs, self.spLocalPub)
def arm(): try: ret = command.arming(value=True) except rospy.ServiceException as ex: fault(ex) if not ret.success: fault('Request failed. Check mavros logs') return ret
def reset_position(self): # Arm the drone mavros.set_namespace() command.arming(True) rospy.wait_for_service('mavros/set_mode') change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode) self.waypoint = PoseStamped() self.waypoint.header.frame_id = 'map' self.waypoint.pose.position.x = self.p_init.x self.waypoint.pose.position.y = self.p_init.y self.waypoint.pose.position.z = self.p_init.z while not rospy.is_shutdown() and (np.linalg.norm( np.array([self.p_init.x - self.p.x, self.p_init.y - self.p.y])) > 0.2 or abs(self.p_init.z - self.p.z > 0.05)): # or \ #rospy.Duration.from_sec(rospy.get_time() - self.t_last_rl_msg.to_sec()).to_sec() > 0.05): result_mode = change_mode(0, "OFFBOARD") self.pub_posmsg.publish(self.waypoint) self.rate.sleep()
def _arm(): """ for this use case, state is always expected to be 'throttle' """ state = 1 # true, throttle on try: ret = command.arming(value=state) except rospy.ServiceException as ex: fault(ex) if not ret.success: fault("Request failed. Check mavros logs") #print_if(args.verbose, "Command result:", ret.result) print("Command result:", ret.result) return ret
def disarm(self): if not self.armed: print("Vehicle Already Disarmed !") else: print("Disarming Now...") try: ret = command.arming(False) self.armed = False if not ret.success: utils.fault("Request failed. Check mavros logs") except rospy.ServiceException as ex: utils.fault(ex) print("Disarming Command result: %s" % (self.armed == False))
def _arm(args, state): try: ret = command.arming(value=state) except rospy.ServiceException as ex: fault(ex) if not ret.success: fault("Request failed. Check mavros logs") print_if(args.verbose, "Command result:", ret.result) return ret # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def __init__(self): # Arm the drone mavros.set_namespace() command.arming(True) self.pub_sp = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) rospy.wait_for_service('mavros/set_mode') self.change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode) if rospy.is_shutdown(): rospy.init_node('gotowaypoint', anonymous=True) self.rate = rospy.Rate(60) # 10hz rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_cb) self.local_pose = PoseStamped() self.waypoint = PoseStamped() self.waypoint.header.frame_id = 'map' self.marker_pub = rospy.Publisher('/waypoint_marker', Marker, queue_size=10) self.waypoint_marker = Marker() self.waypoint_marker.type = Marker.CUBE self.waypoint_marker.header.frame_id = 'map' self.waypoint_marker.scale.x = 0.1 self.waypoint_marker.scale.y = 0.1 self.waypoint_marker.scale.z = 0.1 self.waypoint_marker.color.r = 0.1 self.waypoint_marker.color.g = 1 self.waypoint_marker.color.b = 0 self.waypoint_marker.color.a = 0.6
def __init__(self): rospy.init_node('offboard_test', anonymous=True) pose_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10) tag_pose = rospy.Subscriber('/tag_detections_pose', PoseArray, callback=self.tag_pose_cb) mocap_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, callback=self.mocap_cb) state_sub = rospy.Subscriber('/mavros/state', State, callback=self.state_cb) rate = rospy.Rate(10) # Hz rate.sleep() self.des_pose = self.copy_pose(self.curr_pose) current_x = self.curr_pose.pose.position.x current_y = self.curr_pose.pose.position.y del_val = 0.2 pre_z = 10 while not rospy.is_shutdown(): if self.isReadyToFly: des_z = pre_z if self.tagDetected: if abs(self.x_cam) > 0.2: des_y = current_y - del_val * self.x_cam print "Correcting..", self.x_cam, self.y_cam, des_z if abs(self.y_cam) > 0.2: des_x = current_x - del_val * self.y_cam print "Correcting..", self.x_cam, self.y_cam, des_z if abs(self.x_cam) < 0.2 and abs(self.y_cam) < 0.2: self.tagDetected = False pre_z = des_z - 0.1 * des_z if pre_z < 2: mavros.set_namespace() command.arming(False) print "Arming", des_z des_z = pre_z print "Corrected", des_z else: des_y = current_y des_x = current_x print "Else", des_x, des_y, des_z self.des_pose.pose.position.x = des_x self.des_pose.pose.position.y = des_y self.des_pose.pose.position.z = des_z curr_x = self.curr_pose.pose.position.x curr_y = self.curr_pose.pose.position.y curr_z = self.curr_pose.pose.position.z dist = math.sqrt((curr_x - des_x) * (curr_x - des_x) + (curr_y - des_y) * (curr_y - des_y) + (curr_z - des_z) * (curr_z - des_z)) if dist < self.distThreshold: current_x = curr_x current_y = curr_y mavros.set_namespace() command.arming(False) print "HI" # print dist, curr_x, curr_y, curr_z, self.waypointIndex pose_pub.publish(self.des_pose) rate.sleep()
#!/usr/bin/env python import mavros from mavros import command mavros.set_namespace() # arming command.arming(True) # takeoff command.takeoff(altitude=5, latitude=0, longitude=0, min_pitch=0, yaw=0) # disarming command.arming(False)
Xval_ep = [] Uval_ep = [] tval_ep = [] track_error = [] ctrl_effort = [] print('Starting episodic learning...') for ep in range(Nep + 1): # Run single landing with no perturbation for validation plots: print("Executing trajectory with no perturbation noise...") print("Resetting to initial point...") handler.ctrl_history = [] handler.time_history = [] handler.pert_noise = 0. command.arming(True) go_waypoint.gopoint(np.array(p_init)) X, p_final, U, Upert, t = bintel.gotopoint(p_init, p_final, duration_low, controller=handler) #land() X_val, Xd_val, U_val, _, t_val, ctrl_hist = handler.clean_data( X, p_final, U, Upert, t, ctrl_hist=handler.ctrl_history) handler.pert_noise = pert_noise ctrl_history_ep.append(ctrl_hist) handler.plot_control_ep() # Run training loop: X_w = []
def arm(self): rospy.loginfo("Arming pixhawk...") command.arming(True) self.armed = True
def disarm(self): rospy.loginfo("Disarming pixhawk...") command.arming(False) self.armed = False