def set_offboard_mode(self): done_evt = threading.Event() def state_cb(state): if state.armed: self.state_mach = self.ARMED rospy.loginfo("Drone armed!") elif state.mode == "OFFBOARD": self.state_mach = self.OFFBOARD rospy.loginfo("Drone in OFFBOARD mode!") done_evt.set() else: self.state_mach = self.DISARMED rospy.loginfo("Drone disarmed!") try: set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode) ret = set_mode(custom_mode="OFFBOARD") except rospy.ServiceException as ex: fault(ex) sub = rospy.Subscriber(mavros.get_topic('state'), State, state_cb) if not ret.mode_sent: fault("Request failed. Check mavros logs") if not done_evt.wait(5): fault("Timed out!")
def test_mixed_rates(self): pub = rospy.Publisher("wolf_twist", Twist, queue_size=0) yaw_rate = rospy.wait_for_message(mavros.get_topic("rc", "override"), OverrideRCIn) yaw_rate = rospy.wait_for_message(mavros.get_topic("rc", "override"), OverrideRCIn) twist_out = Twist() twist_out.angular.z = 0.1 twist_out.linear.x = 0.1 twist_out.linear.z = 0.1 pub.publish(twist_out) yaw_rate = rospy.wait_for_message(mavros.get_topic("rc", "override"), OverrideRCIn).channels[3] lateral_rate = rospy.wait_for_message( mavros.get_topic("rc", "override"), OverrideRCIn).channels[4] vertical_rate = rospy.wait_for_message( mavros.get_topic("rc", "override"), OverrideRCIn).channels[2] self.assertEqual( yaw_rate, 1550.0, "PIXHAWK NODE: RC rate does not match sent rate, was {} should be {}" .format(yaw_rate, 1550)) self.assertEqual( lateral_rate, 1550.0, "PIXHAWK NODE: RC rate does not match sent rate, was {} should be {}" .format(lateral_rate, 1550)) self.assertEqual( vertical_rate, 1550.0, "PIXHAWK NODE: RC rate does not match sent rate, was {} should be {}" .format(vertical_rate, 1550))
def write(self, file_, parameters): def to_type(x): if isinstance(x, float): return 9 # REAL32 elif isinstance(x, int): return 6 # INT32 else: raise ValueError("unknown type: " + repr(type(x))) sysid = rospy.get_param(mavros.get_topic('target_system_id'), 1) compid = rospy.get_param(mavros.get_topic('target_component_id'), 1) writer = csv.writer(file_, self.CSVDialect) writer.writerow(("# NOTE: " + time.strftime("%d.%m.%Y %T"), )) writer.writerow( ("# Onboard parameters saved by mavparam for ({}, {})".format( sysid, compid), )) writer.writerow( ("# MAV ID", "COMPONENT ID", "PARAM NAME", "VALUE", "(TYPE)")) for p in parameters: writer.writerow(( sysid, compid, p.param_id, p.param_value, to_type(p.param_value), )) # XXX
def __init__(self): rospy.init_node('listener_results', anonymous=True) self.rate = rospy.Rate(20.0) # MUST be more then 2Hz mavros.set_namespace('mavros') state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, self.state_cb) # "/mavros/setpoint_velocity/cmd_vel" vel_pub_sub = rospy.Subscriber( mavros.get_topic('setpoint_velocity', 'cmd_vel'), TwistStamped, self.vel_pub_cb) vel_local_sub = rospy.Subscriber( mavros.get_topic('local_position', 'velocity_local'), TwistStamped, self.vel_local_cb) self.current_state = mavros_msgs.msg.State() self.vel_pub = [0.0] * 4 self.vel_local = [0.0] * 4 self.vel_pub_x = [] self.vel_pub_y = [] self.vel_pub_z = [] self.vel_local_x = [] self.vel_local_y = [] self.vel_local_z = [] #rospy.spin() self.show() self.plot()
def __init__(self, namespace="mavros"): self.rate = rospy.Rate(20) self.current_pose = PoseStamped() self.current_velocity = TwistStamped() self.target_pose = PoseStamped() self.UAV_state = mavros_msgs.msg.State() mavros.set_namespace(namespace) # setup subscriber self._state_sub = rospy.Subscriber(mavros.get_topic('state'), State, self._state_callback) self._local_position_sub = rospy.Subscriber( mavros.get_topic('local_position', 'pose'), PoseStamped, self._local_position_callback) self._local_velocity_sub = rospy.Subscriber( mavros.get_topic('local_position', 'velocity_body'), TwistStamped, self._local_velocity_callback) # setup publisher self._setpoint_local_pub = mavros.setpoint.get_pub_position_local( queue_size=10) # setup service self.set_arming = rospy.ServiceProxy(mavros.get_topic('cmd', 'arming'), mavros_msgs.srv.CommandBool) self.set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), mavros_msgs.srv.SetMode)
def offboard(self): if self.armed: rate = rospy.Rate(20.0) # MUST be more then 2Hz set_mode_client = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode) local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped, queue_size=10) pose = PoseStamped() pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = self.altitude last_request = rospy.get_rostime() while not rospy.is_shutdown(): now = rospy.get_rostime() if self.mode != "OFFBOARD" and (now - last_request > rospy.Duration(5)): set_mode_client(base_mode=0, custom_mode="OFFBOARD") rospy.loginfo("Entering OFFBOARD MODE NOW ! Use local waypoints for flying") last_request = now pose.header.stamp = rospy.Time.now() pose.pose.position.x = pose.pose.position.x + 0.05 local_pos_pub.publish(pose) rate.sleep() else: rospy.loginfo("Vehicle is not armed, please arm first then proceed !")
def __init__(self): # Setup the nodes and the namespace # If the drone is a slave generate the master ID rospy.init_node("uav_pos_services", anonymous=True) self.rate = rospy.Rate(20) mavros.set_namespace('mavros') # Initialize the parameters self.local_pos = [0.0] * 4 self.global_pos = [0.0] * 4 self.UAV_state = mavros_msgs.msg.State() # setup subscribers # /mavros/state state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, self._state_callback) # /mavros/local_position/pose local_position_sub = rospy.Subscriber( mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self._local_position_callback) # /mavros/global_position/global global_position_sub = rospy.Subscriber('mavros/global_position/global', NavSatFix, self._global_position_callback) # setup publisher # /mavros/setpoint/position/local self.setpoint_local_pub = mavros.setpoint.get_pub_position_local( queue_size=10) # setup the services # /mavros/cmd/arming self.set_arming = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool) # /mavros/set_mode self.set_mode = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode) # Setup global and local service server sg = rospy.Service("target_global_pos", target_global_pos, self.GotoGlobPos) sl = rospy.Service("target_local_pos", target_local_pos, self.GotoLocPos) self.setpoint_msg = mavros.setpoint.PoseStamped( header=mavros.setpoint.Header(frame_id="att_pose", stamp=rospy.Time.now()), ) rospy.loginfo("The target services were successfully initiated") rospy.spin()
def __init__(self): self.local_pos = [0.0] * 4 rospy.init_node("uav_node") self.rate = rospy.Rate(20) mavros.set_namespace('mavros') signal.signal(signal.SIGINT, self.signal_handler) self.UAV_state = mavros_msgs.msg.State() # setup subscribers # /mavros/state state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, self._state_callback) # /mavros/imu/data rospy.Subscriber('/mavros/imu/data', Imu, self.IMU_callback) # # /mavros/local_position/pose local_position_sub = rospy.Subscriber( mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self._local_position_callback) # /mavros/setpoint_raw/target_local setpoint_local_sub = rospy.Subscriber( mavros.get_topic('setpoint_raw', 'target_local'), mavros_msgs.msg.PositionTarget, self._setpoint_position_callback) # setup services # /mavros/cmd/arming self.set_arming = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool) # mavros/cmd/command self.command_srv = rospy.ServiceProxy('mavros/cmd/command', mavros_msgs.srv.CommandLong) # /mavros/set_mode self.set_mode = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode) # goto_pos_services self.goto_loc_pos_serv = rospy.ServiceProxy("target_local_pos", target_local_pos) self.goto_pid_pos_serv = rospy.ServiceProxy("goto_pid_service", goto_pid) # aruco based services # self.goto_aruco_serv = rospy.ServiceProxy('goto_aruco', goto_aruco) # self.land_aruco_serv = rospy.ServiceProxy('land_aruco', land_aruco) # wait for FCU connection while (not self.UAV_state.connected): self.rate.sleep() rospy.loginfo("Uav was successfully connected") self.InputHandler()
def __init__(self): self.current_pose = _vector3() self.setpoint_pose = _vector3() self.mode = "None" self.arm = "None" # setup local_position sub self.local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self._local_position_callback) self.setpoint_local_sub = rospy.Subscriber(mavros.get_topic('setpoint_position', 'pose'), SP.PoseStamped, self._setpoint_position_callback) pass
def param_get_all(force_pull=False): try: pull = rospy.ServiceProxy(mavros.get_topic('param', 'pull'), ParamPull) ret = pull(force_pull=force_pull) except rospy.ServiceException as ex: raise IOError(str(ex)) if not ret.success: raise IOError("Request failed.") params = rospy.get_param(mavros.get_topic('param')) return (ret.param_received, sorted((Parameter(k, v) for k, v in params.iteritems()), cmp=lambda x, y: cmp(x.param_id, y.param_id)))
def do_mode(args): base_mode = 0 custom_mode = '' if args.custom_mode is not None: custom_mode = args.custom_mode.upper() if args.base_mode is not None: base_mode = args.base_mode done_evt = threading.Event() def state_cb(state): print_if(args.verbose, "Current mode:", state.mode) if state.mode == custom_mode: print("Mode changed.") done_evt.set() try: set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode) ret = set_mode(base_mode=base_mode, custom_mode=custom_mode) except rospy.ServiceException as ex: fault(ex) if not ret.mode_sent: fault("Request failed. Check mavros logs") if not done_evt.wait(5): fault("Timed out!")
def velocity_init(self): self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.yaw = 0.0 self.velocity_publish = False self.use_pid = True self.pid_alt = pid_controller.PID() self.pid_alt.setPoint(1.0) # publisher for mavros/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber( mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp) try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread") # TODO(simon): Clean this up. self.done = False self.done_evt = threading.Event()
def __init__(self): rospy.init_node('examplePilot') self.subState = 'GPS' self.GPSMode = True self.enable = False self.rate = rospy.Rate(20) self.exampleGPSPub = rospy.Publisher(targetWP_GPS, mavSP.PoseStamped, queue_size=5) self.exampleATTIPub = rospy.Publisher(targetWP_Atti, mavros_msgs.msg.AttitudeTarget, queue_size=5) self.pilotSubstatePub = rospy.Publisher(onB_substateSub, String, queue_size=1) rospy.Subscriber(onB_StateSub, String, self.onStateChange) rospy.Subscriber( mavros.get_topic('local_position', 'pose'), mavSP.PoseStamped, self.onPositionChange ) # uses mavros.get topic to find the name of the current XYZ position of the drone self.currUAVPos = mavSP.PoseStamped() # used for GPS setpoints self.msgSPAtti = mavSP.TwistStamped() # used for Attitude setpoints rospy.loginfo('examplePilot Initialised.')
def __init__(self): if not MAVLINK: mavros.set_namespace() self.last_rcio_power = RCIOPower() self.publisher = rospy.Publisher('/mavros/rc/override', mavros_msgs.msg.OverrideRCIn) self.in_subscriber = rospy.Subscriber('/mavros/rc/in', mavros_msgs.msg.RCIn, callback=log) self.out_subscriber = rospy.Subscriber('/mavros/rc/out', mavros_msgs.msg.RCOut, callback=log) self.arm_function = rospy.ServiceProxy( mavros.get_topic('cmd', 'arming'), mavros_msgs.srv.CommandBool).call alt_hold_msg = mavros_msgs.msg.OverrideRCIn() alt_hold_msg.channels = [ 0xffff, 0xffff, 0xffff, 0xffff, 1500, 0xffff, 0xffff, 0xffff ] self.publisher.publish(alt_hold_msg) else: self.master = mavutil.mavlink_connection("/dev/ttyACM0", baud=57600) print("waiting for heartbeat...") self.master.wait_heartbeat() print("Connected") self.master.mav.request_data_stream_send( master.target_system, self.master.target_component, mavutil.mavlink.MAV_DATA_STREAM_ALL, 4, 1)
def param_set_list(param_list): # 1. load parameters to parameter server for p in param_list: rospy.set_param(mavros.get_topic('param', p.param_id), p.param_value) # 2. request push all try: push = rospy.ServiceProxy(mavros.get_topic('param', 'push'), ParamPush) ret = push() except rospy.ServiceException as ex: raise IOError(str(ex)) if not ret.success: raise IOError("Request failed.") return ret.param_transfered
def __init__(self): self.hdg_subscriber = rospy.Subscriber( "/mavros/global_position/compass_hdg", Float64, self.compass) self.rel_alt = rospy.Subscriber("/mavros/global_position/rel_alt", Float64, self.altitude) self.home_sub = rospy.Subscriber("/mavros/home_position/home", HomePosition, self.setHomeGeoPointCB) self.gps_sub = rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.current_position_cb) self.state_sub = rospy.Subscriber(mavros.get_topic('state'), State, self.state_cb) self.img_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.call, buff_size=2**24, queue_size=1) self.mono_sub = rospy.Subscriber("/camera/mono/image_raw", Image, self.mono_cb, buff_size=2**24, queue_size=1) self.tree = (self.achieve | ( (self.achieve_alt | self.up) >> (self.yaw_correction | self.correction) >> self.forward )) >> ( self.ah | (self.isdetect >> self.change_alt) ) #>> (self.realsense_check | self.change_alt | (self.mono_detect >> self.change_alt))
def __init__(self): # Reqired call - I think if you are running more than one drone it allows you to give them different topic namespaces, but defaults to mavros mavros.set_namespace() self._curr_state = State() self._is_done = False self.flight_path = [] self.rate = rospy.Rate(20) self.nextPos = PoseStamped() self.lat = 0 self.lng = 0 # Various ros publishers and subscribers - somewhat self documenting self.gps_subscriber = rospy.Subscriber( mavros.get_topic('global_position', 'global'), NavSatFix, self._get_fix) self.state_subscriber = rospy.Subscriber("mavros/state", State, self._update_state) self._local_position_publisher = rospy.Publisher( "mavros/setpoint_position/local", PoseStamped, queue_size=10) self._arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) self._set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode) self._takeoff_client = rospy.ServiceProxy("mavros/cmd/takeoff", CommandTOL)
def __init__(self): self.x = 0.0 self.y = 0.0 self.z = 0.0 self.currentPoseX = 0 self.currentPoseY = 0 self.currentPoseZ = 0 # publisher for mavros/setpoint_position/local self.pub = SP.get_pub_position_local(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.reached) self.waypointsub = rospy.Subscriber("/uav_1/waypoint", SP.PoseStamped, self.updateposition, queue_size=1) try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread") # TODO(simon): Clean this up. self.done = False self.done_evt = threading.Event()
def setParam(bot, update): cmd = update._effective_message.text print cmd logger(bot, update) params = rospy.get_param(mavros.get_topic('param')) newval = float(cmd.split()[3]) print "Set %s => new value %s, old value %s " % ( cmd.split()[1], newval, params.get(cmd.split()[1])) if cmd.split()[1] == "" or cmd.split()[3] == "": update.message.reply_text("Missing data %s => %s " % (cmd.split()[1], newval)) return #pdb.set_trace() if newval == params.get(cmd.split()[1]): print "Already set: %s => %s" % (cmd.split()[1], newval) update.message.reply_text("Already set: %s => %s" % (cmd.split()[1], newval)) return if cmd.split()[1] != "" and newval != "": print "Result: %s" % mavros.param.param_set(cmd.split()[1], newval) update.message.reply_text( "Set %s => new value %s, old value %s " % (cmd.split()[1], newval, params.get(cmd.split()[1])))
def param_get_all(force_pull=False): try: pull = rospy.ServiceProxy(mavros.get_topic('param', 'pull'), ParamPull) ret = pull(force_pull=force_pull) except rospy.ServiceException as ex: raise IOError(str(ex)) if not ret.success: raise IOError("Request failed.") params = rospy.get_param(mavros.get_topic('param')) return (ret.param_received, sorted((Parameter(k, v) for k, v in params.iteritems()), cmp=lambda x, y: cmp(x.param_id, y.param_id)) )
def __init__(self): self.pos_x = 0.0 self.pos_y = 0.0 self.pos_z = 0.0 self.setpoint_x = 0.0 self.setpoint_y = 0.0 self.setpoint_z = 0.0 # publisher for mavros/setpoint_position/pose self.setpoint_position = setpoint.get_pub_position_local(queue_size=1) # subscriber for mavros/local_position/pose self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), setpoint.PoseStamped, self._hasReached, queue_size=1) try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread") # TODO(simon): Clean this up. self.done = False self.done_evt = threading.Event()
def start(): rospy.init_node(name()) mavros.set_namespace() global set_mode, set_param, arming, set_home, takeoff, command, land, wp_push, wp_clear, wp_set set_mode = get_proxy('/mavros/set_mode', SetMode) set_param = get_proxy('/mavros/param/set', ParamSet) arming = get_proxy('/mavros/cmd/arming', CommandBool) set_home = get_proxy('/mavros/cmd/set_home', CommandHome) command = get_proxy('/mavros/cmd/command', CommandLong) takeoff = get_proxy('/mavros/cmd/takeoff', CommandTOL) land = get_proxy('/mavros/cmd/land', CommandTOL) wp_push = get_proxy('/mavros/mission/push', WaypointPush) wp_clear = get_proxy('/mavros/mission/clear', WaypointClear) wp_set = get_proxy('/mavros/mission/set_current', WaypointSetCurrent) global gps_topic, imu_sub gps_topic = mavros.get_topic('global_position', 'global') rospy.Service('command/takeoff', TakeOff, handle_takeoff) rospy.Service('command/land', Land, handle_land) rospy.Service('command/flyto', FlyTo, handle_flyto) rospy.Service('command/fly_waypoints', FlyWaypoints, handle_flywaypoints) rospy.Subscriber("/mavros/global_position/global", NavSatFix, partial(values.latch_value, gps_topic, max_age=10)) # Wait to receive some data from mavros before intializing imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, handle_startup) rospy.spin()
def get_pub_attitude_throttle(**kvargs): """ Returns publisher for :setpoint_attitude: plugin, :cmd_vel: topic """ return rospy.Publisher( mavros.get_topic('setpoint_attitude', 'att_throttle'), Float64, **kvargs)
def __init__(self): self.handlers = [] self.known_events = ['armed', 'disarmed'] self.triggers = {} self.prev_armed = False self.state_sub = rospy.Subscriber(mavros.get_topic('state'), State, self.mavros_state_cb) try: params = rospy.get_param('~') if not isinstance(params, dict): raise KeyError("bad configuration") except KeyError as e: rospy.logerr('Config error: %s', e) return # load configuration for k, v in params.iteritems(): # TODO: add checks for mutually exclusive options if 'service' in v: self._load_trigger(k, v) elif 'shell' in v: self._load_shell(k, v) else: rospy.logwarn("Skipping unknown section: %s", k) # check that events are known rospy.loginfo("Known events: %s", ', '.join(self.known_events)) for h in self.handlers: for evt in h.events: if evt not in self.known_events: rospy.logwarn("%s: unknown event: %s", h.name, evt)
def main(): rospy.init_node('offboardX') rate = rospy.Rate(20) mavros.set_namespace('/mavros') state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, state_callback) arming = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool) #set_mode=rospy.ServiceProxy('/mavros/set_mode',SetMode) set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode) #local_pub = rospy.Publisher(mavros.get_topic('PositionTarget'),mavros_msgs.msg.PositionTarget,queue_size=10) local_pub = rospy.Publisher(mavros.get_topic('PositionTarget'), mavros_msgs.msg.PositionTarget, queue_size=10) pose = PositionTarget() pose.header = Header() pose.header.frame_id = "att_pose" pose.header.stamp = rospy.Time.now() pose.position.x = 0 pose.position.y = 0 pose.position.z = 15 while (not state.connected): rate.sleep() for i in range(0, 50): local_pub.publish(pose) #mavros.command.arming(True) #set_mode(0,'OFFBOARD') last_request = rospy.Time.now() while 1: if (state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0))): print("inside22") if (set_mode(0, 'OFFBOARD').success): print("Offboard enabled") last_request = rospy.Time.now() else: if (not state.armed and (rospy.Time.now() - last_request > rospy.Duration(5.0))): if (mavros.command.arming(True)): print("vehicle armed") last_request = rospy.Time.now() print("entered") local_pub.publish(pose) rospy.spin() rate.sleep() return 0
def __init__(self, actionServer, goal): '''Initialize ros publisher, ros subscriber''' rospack = rospkg.RosPack() packagePath = rospack.get_path('kuri_object_tracking') with open(packagePath+'/config/colors.yaml', 'r') as stream: try: config_yaml = yaml.load(stream) self.H_red = config_yaml.get('H_red') self.S_red = config_yaml.get('S_red') self.V_red = config_yaml.get('V_red') self.H_blue = config_yaml.get('H_blue') self.S_blue = config_yaml.get('S_blue') self.V_blue = config_yaml.get('V_blue') self.H_green = config_yaml.get('H_green') self.S_green = config_yaml.get('S_green') self.V_green = config_yaml.get('V_green') except yaml.YAMLError as exc: print(exc) self.navigate_started = False self.bridge = CvBridge() self.actionServer = actionServer self.obstacles = Objects() self.image_sub = message_filters.Subscriber('/uav_'+str(goal.uav_id)+'/downward_cam/camera/image', Image) self.info_sub = message_filters.Subscriber('/uav_'+str(goal.uav_id)+'/downward_cam/camera/camera_info', CameraInfo) #self.image_sub = message_filters.Subscriber('/usb_cam/image_raw', Image) #self.info_sub = message_filters.Subscriber('/usb_cam/camera_info', CameraInfo) #self.outpub = rospy.Publisher('/uav_'+str(goal.uav_id)+'/downward_cam/camera/detection_image',Image, queue_size=100, latch=True) self.ts = message_filters.TimeSynchronizer([self.image_sub, self.info_sub], 10) self.ts.registerCallback(self.callback) mavros.set_namespace('/uav_'+str(goal.uav_id)+'/mavros') self.currentPoseX = -1 self.currentPoseY = -1 self.currentPoseZ = -1 self.pub = SP.get_pub_position_local(queue_size=10) self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.updatePosition) self.image = None self.left_image = None self.right_image = None self.data_small = np.genfromtxt(packagePath+'/config/small.dat', np.float32, delimiter=',') self.data_big = np.genfromtxt(packagePath+'/config/big.dat', np.float32, delimiter=',') self.data_dropzone = np.genfromtxt(packagePath+'/config/dropzone.dat', np.float32, delimiter=',') self.samples = np.reshape(self.data_small, (-1, 7)) self.samples_big = np.reshape(self.data_big, (-1, 7)) self.samples_dropzone = np.reshape(self.data_dropzone, (-1, 7)) self.objects_count = 0 self.frame = 0 self.new_objects = Objects() self.new_objects_count = 0 self.navigating = False self.done = False self.edgeThresholdSize = 0.1 self.width = 1600 self.height = 1200
def __init__(self): super().__init__() self.mode_data = {} mavros.set_namespace("mavros") # register mode control self.mode_control = rospy.ServiceProxy(mavros.get_topic("set_mode"), SetMode) self.mode_control_type = GraphQLObjectType( "Mode", lambda: { "uuid": GraphQLField(GraphQLString, description="The uuid of the vehicle"), "modeString": GraphQLField(GraphQLString, description=""), "baseMode": GraphQLField(GraphQLInt, description=""), "customMode": GraphQLField(GraphQLInt, description=""), "updateTime": GraphQLField(GraphQLInt, description=""), "returncode": GraphQLField(GraphQLInt, description=""), }, description="Mode control", ) self.q = { "Mode": GraphQLField( self.mode_control_type, args={ "uuid": GraphQLArgument( GraphQLNonNull(GraphQLString), description="The uuid of the target vehicle", ) }, resolve=self.get_mode, ) } self.m = { "Mode": GraphQLField( self.mode_control_type, args=self.get_mutation_args(self.mode_control_type), resolve=self.update_mode, ) } self.s = { "Mode": GraphQLField(self.mode_control_type, subscribe=self.sub_mode, resolve=None) }
def write(self, file_, parameters): def to_type(x): if isinstance(x, float): return 9 # REAL32 elif isinstance(x, int): return 6 # INT32 else: raise ValueError("unknown type: " + repr(type(x))) sysid = rospy.get_param(mavros.get_topic('target_system_id'), 1) compid = rospy.get_param(mavros.get_topic('target_component_id'), 1) writer = csv.writer(file_, self.CSVDialect) writer.writerow(("# NOTE: " + time.strftime("%d.%m.%Y %T"), )) writer.writerow(("# Onboard parameters saved by mavparam for ({}, {})".format(sysid, compid), )) writer.writerow(("# MAV ID" , "COMPONENT ID", "PARAM NAME", "VALUE", "(TYPE)")) for p in parameters: writer.writerow((sysid, compid, p.param_id, p.param_value, to_type(p.param_value), )) # XXX
def __init__(self): self.current_pose = _vector3() self.setpoint_pose = _vector3() self.mode = "None" self.arm = "None" self.guided = "None" self.timestamp = float(datetime.utcnow().strftime('%S.%f')) self.conn_delay = 0.0 rospy.init_node('UAV_Monitor') mavros.set_namespace("/mavros") # setup local_position sub self.local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self._local_position_callback) self.setpoint_local_sub = rospy.Subscriber(mavros.get_topic('setpoint_raw', 'target_local'), mavros_msgs.msg.PositionTarget, self._setpoint_position_callback) self.state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, self._state_callback) pass
def __init__(self): ''' Class that acts as a server for the goto_pid service ''' # init node rospy.init_node('pid_navigation_service', anonymous=True) mavros.set_namespace('mavros') # Setup Subscribers ## mavros state state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, self._state_callback) # Setup publishers # /mavros/setpoint_velocity/cmd_vel self.cmd_vel_pub = rospy.Publisher("mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10) # /mavros/local_position/pose local_position_sub = rospy.Subscriber( mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self._local_position_callback) # setup services # /mavros/cmd/arming self.set_arming = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool) # /mavros/set_mode self.set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode) # Initialize the service servers goto_pid_serv = rospy.Service('goto_pid_service', goto_pid, self.GotoLocPid) # Initialize variables self.local_pos = [0.0] * 4 self.UAV_state = mavros_msgs.msg.State() # Setup rate self.rate = rospy.Rate(20) rospy.sleep(1) rospy.spin()
def set_mode(custom_mode='GUIDED', base_mode=0): print 'Setting mode to (%d, %s)' % (base_mode, custom_mode) try: set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), SetMode) ret = set_mode(base_mode=base_mode, custom_mode=custom_mode) except rospy.ServiceException as ex: fault(ex) if not ret.success: fault('Request failed. Check mavros logs')
def __init__(self): rospy.init_node('pylon_node') rospy.Subscriber(requestSub, Bool, self.cb_onRequest) rospy.Subscriber(mavros.get_topic('global_position', 'global'), NavSatFix, self.cb_onPosUpdate) self.pylonPub = rospy.Publisher(pylonDataPub, pylonList, queue_size=1) self.osmAPI = "https://api.openstreetmap.org/api/0.6/map" self.boundary = 350 #metres -- curpos +/- (boundary / 2) self.dronePos = NavSatFix() self.pylonList = [] rospy.spin()
def param_get(param_id): try: get = rospy.ServiceProxy(mavros.get_topic('param', 'get'), ParamGet) ret = get(param_id=param_id) except rospy.ServiceException as ex: raise IOError(str(ex)) if not ret.success: raise IOError("Request failed.") return param_ret_value(ret)
def main(): # print "TASK: "+str(sys.argv) # setup rospy env rospy.init_node('TCS_task', anonymous=True) rate = rospy.Rate(20) mavros.set_namespace('/mavros') # setup local pub setpoint_local_pub = mavros.setpoint.get_pub_position_local(queue_size=10) # setup setpoint_msg setpoint_msg = mavros.setpoint.PoseStamped( header=mavros.setpoint.Header( frame_id="local_pose", stamp=rospy.Time.now()), ) # setup local sub position_local_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, local_position_cb) # setup task pub task_watchdog = TCS_util.Task_watchdog('TASK_LOCAL_GOTO') # interprete the input position setpoint_arg = sys.argv[1].split(' ') setpoint_position.x=float(setpoint_arg[0]) setpoint_position.y=float(setpoint_arg[1]) setpoint_position.z=float(setpoint_arg[2]) print "X: {}, Y: {}, Z: {}".format(setpoint_position.x, setpoint_position.y, setpoint_position.z) # setup setpoint poisiton and prepare to publish the position set_target(setpoint_msg, setpoint_position.x, setpoint_position.y, setpoint_position.z) # In this while loop, do the job. while(not is_reached()): # When the UAV reached the position, # publish the task finished signal and exit setpoint_local_pub.publish(setpoint_msg) # TODO: publish the task status as conducting task_watchdog.report_running() rate.sleep() # TODO: publish the task status as FINISHING task_watchdog.report_finish() return 0
def wait_fcu_connection(timeout=None): """ Wait until establishing FCU connection """ try: msg = rospy.wait_for_message(mavros.get_topic('state'), mavros.msg.State, timeout) if msg.connected: return True except rospy.ROSException as e: return False connected = threading.Event() def handler(msg): if msg.connected: connected.set() sub = rospy.Subscriber( mavros.get_topic('state'), mavros.msg.State, handler ) return connected.wait(timeout)
def __init__(self): self.x = 0.0 self.y = 0.0 self.z = 0.0 self.pub = SP.get_pub_position_local(queue_size=10) self.sub = rospy.Subscriber( mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.reached) try: thread.start_new_thread( self.navigate, () ) except: fault("Error: Unable to start the thread") self.done = False self.done_evt = threading.Event()
def param_set(param_id, value): if isinstance(value, float): val = ParamValue(integer=0, real=value) else: val = ParamValue(integer=value, real=0.0) try: set = rospy.ServiceProxy(mavros.get_topic('param', 'set'), ParamSet) ret = set(param_id=param_id, value=val) except rospy.ServiceException as ex: raise IOError(str(ex)) if not ret.success: raise IOError("Request failed.") return param_ret_value(ret)
def __init__(self, setpoint_publish): self.setpoint_pub = setpoint_publish self.current_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self._local_position_callback) self.current = vector3() self.msg = mavros.setpoint.PoseStamped( header=mavros.setpoint.Header( frame_id="att_pose", stamp=rospy.Time.now()), ) self.x = 0 self.y = 0 self.z = 0 # default precision is 0.5 self.precision = 0.5
def __init__(self, copter_id = "1"): self.copter_id = copter_id mavros_string = "/mavros" #rospy.init_node('velocity_goto_'+copter_id) mavros.set_namespace(mavros_string) # initialize mavros module with default namespace self.mavros_string = mavros_string self.final_alt = 0.0 self.final_pos_x = 0.0 self.final_pos_y = 0.0 self.final_vel = 0.0 self.cur_rad = 0.0 self.cur_alt = 0.0 self.cur_pos_x = 0.0 self.cur_pos_y = 0.0 self.cur_vel = 0.0 self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.avz = 0.0 self.pose_open = [] self.alt_control = True self.override_nav = False self.reached = True self.done = False self.last_sign_dist = 0.0 # for local button handling self.click = " " self.button_sub = rospy.Subscriber("abpause_buttons", std_msgs.msg.String, self.handle_buttons) # publisher for mavros/copter*/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/copter*/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp) self.cv_bridge = CvBridge() self.image_data = open('image_data.txt','w')
def __init__(self): self.x = 0.0 self.y = 0.0 self.z = 0.0 # publisher for mavros/setpoint_position/local self.pub = SP.get_pub_position_local(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic("local_position", "pose"), SP.PoseStamped, self.reached) try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread") # TODO(simon): Clean this up. self.done = False self.done_evt = threading.Event()
def __init__(self, sonar_obs_present=[], jMAVSim=False ): self.x = 0.0 self.y = 0.0 self.z = 0.0 self.yaw = 0.0 self.jMAVSim = jMAVSim # publisher for mavros/setpoint_position/local self.pub = SP.get_pub_position_local(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.reached) try: #thread.start_new_thread( self.navigate, () ) navigation_thread = threading.Thread(target=self.navigate) navigation_thread.start() except: print "Error: Unable to start thread" self.sonar_obs_present = sonar_obs_present self.obs_detected = False #current coordinates self.curr_x = 0.0 self.curr_y = 0.0 self.curr_z = 0.0 self.x_home = 0 self.y_home = 0 self.z_home = 0 #coordinates for original setpoint, stored to continue to desired setpoint if interrupted self.x_final_dest = 0.0 self.y_final_dest = 0.0 self.z_final_dest = 0.0 self.yaw_final_dest = 0.0 self.done = False self.done_evt = threading.Event()
def __init__(self,actionServer): self.actionServer = actionServer self.done = False self.done_evt = threading.Event() self.isExploring = False self.progress = 0.0 self.x = 0.0 self.y = 0.0 self.z = 0.0 self.currentPoseX = 0 self.currentPoseY = 0 self.currentPoseZ = 0 self.navigating = False # publisher for mavros/setpoint_position/local self.pub = SP.get_pub_position_local(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.reached) self.objects_map = ObjectsMap() self.client = actionlib.SimpleActionClient('MappingAction', MappingAction) client = self.client #client = self.actionServer.client print "Waiting for mapping server" client.wait_for_server() goal = TrackingGoal() goal.uav_id = 3 client.send_goal(goal) print "Waiting for result" client.wait_for_result() print "Result:" self.objects_map = client.get_result().objects_map print self.objects_map self.sub = rospy.Subscriber("MappingAction/feedback",ObjectsMap, self.callback) try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread")
def __init__(self): self.done = False self.done_evt = threading.Event() self.isExploring = False self.progress = 0.0 self.x = 0.0 self.y = 0.0 self.z = 0.0 self.currentPoseX = 0 self.currentPoseY = 0 self.currentPoseZ = 0 self.navigating = False mavros.set_namespace('/uav_1/mavros') # publisher for mavros/setpoint_position/local self.pub = SP.get_pub_position_local(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.reached) self.objects_map = ObjectsMap() self.client = actionlib.SimpleActionClient('TrackingAction', TrackingAction) #client = self.client #client = self.actionServer.client print "Waiting for tracking server" self.client.wait_for_server() self.goal = TrackingGoal() self.goal.uav_id = 1 self.client.send_goal(self.goal) print "Waiting for result" self.client.wait_for_result() print "Result:" self.objects =self.client.get_result().tracked_objects.objects print self.objects try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread")
def __init__(self): self.x = 0.0 self.y = 0.0 self.z = 0.0 self.currentPoseX = 0 self.currentPoseY = 0 self.currentPoseZ = 0 # publisher for mavros/setpoint_position/local self.pub = SP.get_pub_position_local(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self.reached) self.waypointsub = rospy.Subscriber("/uav_1/waypoint", SP.PoseStamped, self.updateposition, queue_size=1) try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread") self.done = False self.done_evt = threading.Event()
def velocity_init(self): self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.yaw = 0.0 self.velocity_publish = False self.use_pid = True self.pid_alt = pid_controller.PID() self.pid_alt.setPoint(1.0) # publisher for mavros/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp) try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread") # TODO(simon): Clean this up. self.done = False self.done_evt = threading.Event()
def __init__(self): self.handlers = [] self.known_events = ['armed', 'disarmed'] self.triggers = {} self.prev_armed = False try: params = rospy.get_param('~') if not isinstance(params, dict): raise KeyError("bad configuration") except KeyError as e: rospy.logerr('Config error: %s', e) return # load configuration for k, v in params.iteritems(): # TODO: add checks for mutually exclusive options if 'service' in v: self._load_trigger(k, v) elif 'shell' in v: self._load_shell(k, v) else: rospy.logwarn("Skipping unknown section: %s", k) # check that events are known rospy.loginfo("Known events: %s", ', '.join(self.known_events)) for h in self.handlers: for evt in h.events: if evt not in self.known_events: rospy.logwarn("%s: unknown event: %s", h.name, evt) # config loaded, we may subscribe self.state_sub = rospy.Subscriber( mavros.get_topic('state'), State, self.mavros_state_cb)
def main(): rospy.init_node('default_offboard', anonymous=True) rate = rospy.Rate(20) mavros.set_namespace('/mavros') # setup subscriber # /mavros/state state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, _state_callback) # /mavros/local_position/pose # local_position_sub = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), # SP.PoseStamped, _local_position_callback) # /mavros/setpoint_raw/target_local setpoint_local_sub = rospy.Subscriber(mavros.get_topic('setpoint_raw', 'target_local'), mavros_msgs.msg.PositionTarget, _setpoint_position_callback) # setup publisher # /mavros/setpoint/position/local setpoint_local_pub = mavros.setpoint.get_pub_position_local(queue_size=10) # setup service # /mavros/cmd/arming set_arming = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool) # /mavros/set_mode set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode) setpoint_msg = mavros.setpoint.PoseStamped( header=mavros.setpoint.Header( frame_id="att_pose", stamp=rospy.Time.now()), ) #read task list Task_mgr = TCS_util.Task_manager('task_list.log') # start setpoint_update instance setpoint_keeper = TCS_util.update_setpoint(rospy) # wait for FCU connection while(not UAV_state.connected): rate.sleep() # initialize the setpoint setpoint_msg.pose.position.x = 0 setpoint_msg.pose.position.y = 0 setpoint_msg.pose.position.z = 3 mavros.command.arming(True) # send 100 setpoints before starting for i in range(0,50): setpoint_local_pub.publish(setpoint_msg) rate.sleep() set_mode(0,'OFFBOARD') last_request = rospy.Time.now() # enter the main loop while(True): # print "Entered whiled loop" if( UAV_state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0))): if( set_mode(0,'OFFBOARD').success): print "Offboard enabled" last_request = rospy.Time.now() else: if(not UAV_state.armed and (rospy.Time.now() - last_request > rospy.Duration(5.0))): if( mavros.command.arming(True)): print "Vehicle armed" last_request = rospy.Time.now() # update setpoint to stay in offboard mode setpoint_keeper.update() if(Task_mgr.task_finished()): # If the current task has been done rospy.loginfo("Current task is finished!") if (not Task_mgr.alldone()): # If there are tasks left Task_mgr.nexttask() else: # Current task has been done and no task left rospy.loginfo("All tasks have been done!") while (UAV_state.mode != "AUTO.LAND"): set_mode(0,'AUTO.LAND') rate.sleep() return 0 rate.sleep() return 0
def start_subs(self): # publisher for mavros/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp)
def get_pub_attitude_posecov(**kvargs): """ Returns publisher for :setpoint_attitude: plugin, :attituse: topic (with covariance) """ return rospy.Publisher(mavros.get_topic('setpoint_attitude', 'attitude'), PoseWithCovarianceStamped, **kvargs)
def get_pub_position_local(**kvargs): """ Returns publisher for :setpoint_position: plugin, :local: topic """ return rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped, **kvargs)
def get_pub_velocity_cmd_vel(**kvargs): """ Returns publisher for :setpoint_velocity: plugin, :cmd_vel: topic """ return rospy.Publisher(mavros.get_topic('setpoint_velocity', 'cmd_vel'), TwistStamped, **kvargs)
def _get_proxy(service, type): return rospy.ServiceProxy(mavros.get_topic('cmd', service), type)
def halt(self, delay=0): self.sub_localpos = rospy.Subscriber(mavros.get_topic('local_position', 'pose'), SP.PoseStamped, self._local_position_callback) time.sleep(delay) print self.curr_x, self.curr_y, self.curr_z setpoint.set(self.curr_x, self.curr_y, self.curr_z, self.yaw)
def subscribe_waypoints(cb, **kvargs): return rospy.Subscriber( mavros.get_topic('mission', 'waypoints'), WaypointList, cb, **kvargs)
def _get_proxy(name, type): return rospy.ServiceProxy(mavros.get_topic('mission', name), type)