Python mavros.get_topic函数代码示例

	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_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 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, 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 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 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 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, 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, 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):
        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()