Python rospy.Publisher類代碼示例

def main_auto():
    # initialize ROS node
    init_node('auto_mode', anonymous=True)
    nh = Publisher('ecu', ECU, queue_size = 10)

	# set node rate
    rateHz  = get_param("controller/rate")
    rate 	= Rate(rateHz)
    dt   	= 1.0 / rateHz
    t_i     = 0

    # get experiment parameters 
    t_0             = get_param("controller/t_0")     # time to start test
    t_f             = get_param("controller/t_f")     # time to end test
    FxR_target      = get_param("controller/FxR_target")
    d_f_target      = pi/180*get_param("controller/d_f_target")
 
    # main loop
    while not is_shutdown():
        # get command signal
        (FxR, d_f) = circular(t_i, t_0, t_f, d_f_target, FxR_target)
			
        # send command signal 
        ecu_cmd = ECU(FxR, d_f)
        nh.publish(ecu_cmd)
	
        # wait
        t_i += dt
        rate.sleep()

class Program:
    def main(self):
        if len(sys.argv) >= 3:
            rospy.init_node("RiC_PPM")
            topicNamePmm = sys.argv[1]
            topicNameDiff = sys.argv[2]
            # print 'PPM Topic: ' + topicNamePmm + '\n' + 'Diff Topic: ' + topicNameDiff
            self.pub = Publisher(topicNameDiff, Twist,queue_size=1)
            Subscriber(topicNamePmm, PPM, self.callBack, queue_size=1)
            rospy.spin()

    def callBack(self, msg):

        leftAndRight = 0
        upAndDown = 0

        if 1450 < msg.channels[0] < 1550: leftAndRight = 0
        else: leftAndRight = 0.032 * (float(msg.channels[0])) - 48

        if 1450 < msg.channels[1] < 1550: upAndDown = 0
        else: upAndDown = 0.032 * (float(msg.channels[1])) - 48

        msgPub = Twist()
        msgPub.angular.z = leftAndRight
        msgPub.linear.x = -upAndDown

        self.pub.publish(msgPub)

class RiCBattery(Device):
    def __init__(self, param, output):
        Device.__init__(self, param.getBatteryName(), output)
        self._pub = Publisher('%s' % self._name, Float32, queue_size=param.getBatteryPubHz())
        self._haveRightToPublish = False
        #KeepAliveHandler(self._name, Float32)

    def publish(self, data):
        msg = Float32()
        msg.data = data
        self._pub.publish(msg)

    def getType(self): return Battery

    def checkForSubscribers(self):
        try:
            subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]

            if not self._haveRightToPublish and subCheck == '':
                self._output.write(PublishRequest(Battery, 0, True).dataTosend())
                self._haveRightToPublish = True

            elif self._haveRightToPublish and subCheck == 'None':
                self._output.write(PublishRequest(Battery, 0, False).dataTosend())
                self._haveRightToPublish = False
        except: pass

class RiCPPM(Device):
    def __init__(self, param, output):
        Device.__init__(self, param.getPPMName(), output)
        self._pub = Publisher('%s' % self._name, PPM, queue_size=param.getPPMPubHz())
        self._haveRightToPublish = False

    def getType(self): return PPM

    def publish(self, data):
        msg = PPM()
        msg.channels = data.getChannels()
        self._pub.publish(msg)

    def checkForSubscribers(self):
        try:
            subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]

            if not self._haveRightToPublish and subCheck == '':
                self._output.write(PublishRequest(7, 0, True).dataTosend())
                self._haveRightToPublish = True

            elif self._haveRightToPublish and subCheck == 'None':
                self._output.write(PublishRequest(7, 0, False).dataTosend())
                self._haveRightToPublish = False
        except: pass

class RiCSwitch(Device):
    def __init__(self, devId,param, output):
        Device.__init__(self, param.getSwitchName(devId), output)
        self._pub = Publisher('%s' % self._name, Bool, queue_size=param.getSwitchPubHz(devId))
        self._switchId = devId
        self._haveRightToPublish = False

    def publish(self, data):
        msg = Bool()
        msg.data = data
        self._pub.publish(msg)

    def checkForSubscribers(self):
        try:
            subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1]

            if not self._haveRightToPublish and subCheck == '':
                self._output.write(PublishRequest(Button, self._switchId, True).dataTosend())
                self._haveRightToPublish = True

            elif self._haveRightToPublish and subCheck == 'None':
                self._output.write(PublishRequest(Button, self._switchId, False).dataTosend())
                self._haveRightToPublish = False
        except: pass

    def getType(self): return Button

 def __init__(self, param, output):
     Device.__init__(self, param.getIMUName(), output)
     self._frameId = param.getIMUFrameId()
     self._angle = param.getIMUOrientation()
     self._pub = Publisher("%s_AGQ" % self._name, Imu, queue_size=param.getIMUPubHz())
     self._pubMag = Publisher("%s_M" % self._name, MagneticField, queue_size=param.getIMUPubHz())
     Service("%s_Calibration" % self._name, calibIMU, self.serviceCallBack)
     self._haveRightToPublish = False

def random_temperatures(delay=1):
    print('Starting random temperatures...')
    pub = Publisher('/cpu/temperature', Temperature)
    msg = Temperature()
    msg.name = ['cpu0', 'cpu1', 'cpu2', 'cpu2']
    while not rospy.is_shutdown():
        msg.temperature = [random.randint(30, 80) for i in range(4)]
        sleep(delay)
        pub.publish(msg)

def random_co2(delay=1):
    print('Starting random battery data...')
    pub = Publisher('/sensors/co2', Co2Msg)
    msg = Co2Msg()
    while not rospy.is_shutdown():
        msg.header = rospy.Header()
        msg.co2_percentage = random.random() * 10
        sleep(delay)
        pub.publish(msg)

def random_thermal(delay=1):
    print('Starting random thermal data...')
    pub = Publisher('/sensors/thermal', ThermalMeanMsg)
    msg = ThermalMeanMsg()
    while not rospy.is_shutdown():
        msg.header = rospy.Header()
        msg.thermal_mean = random.randint(20, 40)
        sleep(delay)
        pub.publish(msg)

def random_imu(delay=1):
    print('Starting random imu...')
    pub = Publisher('/sensors/imu_rpy', ImuRPY)
    msg = ImuRPY()
    while not rospy.is_shutdown():
        msg.roll = random.random() * 50
        msg.pitch = random.random() * 50
        msg.yaw = random.random() * 50
        pub.publish(msg)
        sleep(delay)

def random_battery(delay=1):
    print('Starting random battery data...')
    pub = Publisher('/sensors/battery', BatteryMsg)
    msg = BatteryMsg()
    msg.name = ['PSU', 'Motors']
    while not rospy.is_shutdown():
        battery1 = random.randint(18, 25)
        battery2 = random.randint(18, 25)
        msg.voltage = [battery1, battery2]
        sleep(delay)
        pub.publish(msg)

def random_sonar(delay=1):
    print('Starting random sonar...')
    pub = Publisher('/sensors/range', Range)
    msg = Range()
    while not rospy.is_shutdown():
        msg.header = rospy.Header(frame_id='right_sonar_frame')
        msg.range = random.random() * 20
        pub.publish(msg)
        sleep(delay)
        msg.header = rospy.Header(frame_id='left_sonar_frame')
        msg.range = random.random() * 20
        pub.publish(msg)

def quad_controller(route_queue, position_queue, arming_queue,
                    trgt_queue, waypoints_queue):
    flight_done_pub = Publisher('remove', UInt32, queue_size=1)
    while not is_shutdown():
        # Take a route
        route = route_queue.get(True, None)
        loginfo('Received route with id #'+str(route.id)+'.')
        if not route.valid:
            logerr('Route invalid!')
            return
        target_pos = trgt_queue.get(True, None)
        need_waypoint_id = None
        
        # Determine the point where you want to dump the load
        need_waypoint_id = find_waypoint_id(route, (target_pos.latitude, target_pos.longitude))
        loginfo('need_waypoint_id: ' + str(need_waypoint_id))

        # Push a mission into flight controller
        push_mission(waypointWrap(route.route))
        loginfo('Mission loaded to flight controller.')
        # Set manual mode
        set_mode('ACRO')
        # Enable motors
        arming()
        # Set autopilot mode
        set_mode('AUTO')
        loginfo('Flight!')

        # Wainting for arming
        sleep(5)
        drop_all(arming_queue)

        # If you do not have a goal to clear cargo,
        # we believe that already dropped
        droped = (need_waypoint_id == None)
        while arming_queue.get().data and not droped:
            waypoints = None
            # To not accumulate elements in arming_queue
            try:
                waypoints = waypoints_queue.get_nowait().waypoints
            except Empty:
                continue
            current_waypoint_id = find_cur_waypoint_id(waypoints)
            if current_waypoint_id > need_waypoint_id:
                droped = True
                drop_cargo()

        # TODO: More elegant case for mission finish check
        while arming_queue.get().data:
            pass

        loginfo('Mission #'+str(route.id)+' complete!')
        flight_done_pub.publish(route.id)

class ScanSensor:
    __MIN_ANGLE = 0.0
    __MAX_ANGLE = 2.0*pi
    __ANGLE_INCREMENT = 2.0*pi/360.0

    def __init__(self, pub_name, frame_id, min_range, max_range):

        self._publisher = Publisher(pub_name, LaserScan, queue_size=10)

        self._frame_id = frame_id
        self._min_range = min_range
        self._max_range = max_range
        self._last_time = Time.now()

        try:
            self._hal_proxy = BaseHALProxy()
        except BaseHALProxyError:
            raise ScanSensorError("Unable to create HAL")

    def _get_data(self):
        """
        This method is overridden in the subclasses and returns a tuple of ranges (distances), intensities, and delta time
        """
        return [], [], 0

    def _msg(self, ranges, intensities, scan_time):
        new_time = Time.now()
        delta_time = new_time - self._last_time
        self._last_time = new_time

        msg = LaserScan()
        msg.header.frame_id = self._frame_id
        msg.header.stamp = Time.now()
        msg.angle_max = self.__MAX_ANGLE
        msg.angle_min = self.__MIN_ANGLE
        msg.angle_increment = self.__ANGLE_INCREMENT
        # Note: time_increment is the time between measurements, i.e. how often we read the scan and publish it (in
        # seconds)
        msg.time_increment = 0.1 #delta_time.secs
        # Note: scan_time is the time between scans of the laser, i.e., the time it takes to read 360 degrees.
        msg.scan_time = 0.1 # scan_time
        msg.range_min = float(self._min_range)
        msg.range_max = float(self._max_range)
        msg.ranges = [min(max(range, msg.range_min), msg.range_max) for range in ranges]
        msg.intensities = intensities

        return msg

    def Publish(self):
        ranges, intensities, scan_time  = self._get_data()
        self._publisher.publish(self._msg(ranges, intensities, scan_time))

class Estimator():

    """
    Estimator Node: publishes final result once a second.

    Accumulate predictions and publishes a final result every second
    """

    def __init__(self, **kwargs):
        """Constructor."""
        rospy.init_node('ocular_object_id_averager', anonymous=True)
        self.node_name = rospy.get_name()
        rospy.on_shutdown(self.shutdown)
        loginfo("Initializing " + self.node_name + " node...")

        with eh(logger=logfatal, log_msg="Couldn't load parameters",
                reraise=True):
            self.hz = load_params(['rate']).next()

        # Publishers and Subscribers
        self.pub = Publisher('final_object_id', SystemOutput)
        Subscriber("object_id", RecognizedObject, self.callback)

        # self.r = rospy.Rate(self.hz)
        # Accumulates Hz items in per second. Ex: 30Hz -> ~30items/sec
        self.accumulator = Accumulator(self.hz)

    def callback(self, data):
        """Callback that publishes updated predictions when new msg is recv."""
        self.accumulator.append(data.object_id)
        if self.accumulator.isfull():
            rospy.logdebug("Accumulator full. Printing all predictions")
            rospy.logdebug("{}".format(self.accumulator))
            predictions_rgb, predictions_pcloud = zip(*self.accumulator)
            y, y_rgb, y_pcloud = estimate(predictions_rgb, predictions_pcloud)
            output_msg = SystemOutput(id_2d_plus_3d=y,
                                      id_2d=y_rgb,
                                      id_3d=y_pcloud)
            try:
                self.pub.publish(output_msg)
            except ValueError as ve:
                rospy.logwarn(str(ve))

    def run(self):
        """Run (wrapper of ``rospy.spin()``."""
        rospy.spin()

    def shutdown(self):
        """Shudown hook to close the node."""
        loginfo('Shutting down ' + rospy.get_name() + ' node.')