Python Range.range方法代码示例

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
def main():
    global NUM_SENSORS, BAUD_RATE, DEVICE_ADDRESS
    rospy.loginfo("Launching arduino analog reader")
    rospy.init_node("arduinoReader")

    ser = serial.Serial(DEVICE_ADDRESS)
    ser.baudrate = BAUD_RATE

    pubs = []
    for index in range(NUM_SENSORS):
        pubs.append(rospy.Publisher(("/sonar/" + str(index)), Range))

    while not rospy.is_shutdown():
        data = ser.readline().split("\t")
        data.pop() # last element is garbage
        rospy.loginfo("Read line: " + str(data))
        for reading in data:
            #construct message
            msg = Range()
            try:
                msg.range = float(reading)
            except ValueError:
                msg.range = -1.0

            pubIndex = data.index(reading)
            if pubIndex < NUM_SENSORS :
                pubs[pubIndex].publish(msg)

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
	def test_range(self):
		sensor = Sensors.Range(min_safe=10)
		msg = Range()
		msg.max_range = 3000
		msg.min_range = 5
		msg.range = 2500

		sensor.measure(msg)

		self.assertEquals(sensor.max_range, msg.max_range)
		self.assertEquals(sensor.min_range, msg.min_range)
		self.assertEquals(sensor.range , msg.range)
		self.assertFalse(sensor.isFar())
		self.assertFalse(sensor.isNear())

		msg.range = 4001
		sensor.measure(msg)
		self.assertTrue(sensor.isFar())
		self.assertFalse(sensor.isNear())

		msg.range = 3
		sensor.measure(msg)
		self.assertFalse(sensor.isFar())
		self.assertTrue(sensor.isNear())

		msg.range = 8
		sensor.measure(msg)
		self.assertFalse(sensor.isFar())
		self.assertTrue(sensor.isNear())

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
def range_test():
	from sensor_msgs.msg import Range 
	sensor = RangeSensor()
	print sensor

	m = Range()
	m.range = 2000.0
	m.max_range = 3000.0
	m.min_range = 2.01

	sensor.min_safe = 500

	sensor.measure(m)

	print sensor 
	print sensor.isNear()
	print sensor.isFar()

	m.range = 4000
	sensor.measure(m)
	print sensor 
	print sensor.isNear()
	print sensor.isFar()

	m.range = -2000
	sensor.measure(m)
	print sensor 
	print sensor.isNear()
	print sensor.isFar()

	m.range = 300
	sensor.measure(m)
	print sensor 
	print sensor.isNear()
	print sensor.isFar()

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
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)

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
def talker():
    signal.signal(signal.SIGINT, signal_handler)
    time.sleep(1)
    setup_servo()
    pub = rospy.Publisher('servo_ulta', Range, queue_size=10)
    rospy.init_node('servo_ultra', anonymous=True, disable_signals=False )
    rate = rospy.Rate(10) # 10hz
    ultrasonic_number = 1
    
    pause_time = 0.1
    delay_ultrasonic = 0.11
    STEP_ANGLE = 10

    CURR_ANGLE = (90.0 * math.pi) / 180.0

    mindt = 0.65
    maxdt = 2.48
    dt = [mindt, maxdt]
    extremes = [int(x * 1024.0 / 20.0) for x in dt]

    dt_range = extremes[1] - extremes[0]
    dt_step = int((dt_range / 180.0) * STEP_ANGLE)

    br = tf.TransformBroadcaster()

    while not rospy.is_shutdown():
        #broadcast_transforms()

        # Move servo to counter-clockwise extreme.
        wiringpi.pwmWrite(18, extremes[1])
        sleep(0.2)
        CURR_ANGLE = (90.0 * math.pi) / 180.0

        for i in range(extremes[1],extremes[0],-dt_step):         # 1025 because it stops at 1024
            wiringpi.pwmWrite(18,i)
            sleep(pause_time)
            br.sendTransform((0.15, 0.0, 0.0),tf.transformations.quaternion_about_axis(CURR_ANGLE, (0, 0, 1)), rospy.Time.now(), "1", "base_link")
            data = Range()
            data.header.frame_id = "1"
            data.header.stamp = rospy.Time.now()
            data.radiation_type = 0
            data.min_range = 0.05 
            data.max_range = 2.0
            data.field_of_view = 0.164
            try :
                data.range = float(get_us_data_from(ultrasonic_number)) /100.0
            except IOError:
                subprocess.call(['i2cdetect', '-y', '1'])
                data.range = 0.0
            pub.publish(data)
            CURR_ANGLE -= (STEP_ANGLE * math.pi) / 180.0

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
def callDist(data):

		#reads voltage value
        voltage = ADC.read("P9_40")
		#converts voltage values into distance(meters)
        distance = (voltage**-.8271732796)
        distance = distance*.1679936709
    	#checks and discards data outside of accurate range
        if distance>2:
            distance = 2
        elif distance<.15:
            distance = .15

    	#Writes data to Range message
        msg = Range()
        header = Header()
        #creates header
        msg.header.stamp.secs = rospy.get_time()
        msg.header.frame_id = "/base_link"
        msg.radiation_type = 1
        msg.field_of_view = .15	#about 8.5 degrees
        msg.min_range = .15
        msg.max_range = 2
        msg.range = distance
        pub.publish(msg)

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
    def Publish(self):
		#初始化节点
        rospy.init_node("talkerUltraSound", anonymous = True)        
        #超声波数据发布节点
        self.ultrasound_pub = rospy.Publisher("UltraSoundPublisher", Range, queue_size = 20)
        
        ran_broadcaster = tf.TransformBroadcaster()
        
        rate=rospy.Rate(10)
        #print "Input distance('+' is the end of the number):"
        while not rospy.is_shutdown():
			#ch=self.getKey()
			#self.addChar(ch)
			ran_quat = Quaternion()
			ran_quat = quaternion_from_euler(0, 0, 0)	
			#发布TF关系
			ran_broadcaster.sendTransform((0.2,0.0,0.2),ran_quat,rospy.Time.now(),"ultrasound","base_link")
			#定义一个超声波消息
			ran = Range()
			ran.header.stamp = rospy.Time.now()
			ran.header.frame_id = "/ultrasound"
			ran.field_of_view = 1;
			ran.min_range = 0;
			ran.max_range = 5;
			ran.range = self.dist;
			#ultrasound_pub.publish(ran)
			rate.sleep()		

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
    def __init__(self):
        #初始化节点
        rospy.init_node("talkerUltraSound", anonymous = True)        
        #超声波数据发布节点
        ultrasound_pub = rospy.Publisher("UltraSoundPublisher", Range, queue_size = 20)
        
        ran_broadcaster = tf.TransformBroadcaster()
        
        rate=rospy.Rate(1)
        
        while not rospy.is_shutdown():
			ran_quat = Quaternion()
			ran_quat = quaternion_from_euler(0, 0, 0)	
			#发布TF关系
			ran_broadcaster.sendTransform((0.2,0.0,0.2),ran_quat,rospy.Time.now(),"ultrasound","base_link")
			#定义一个超声波消息
			ran = Range()
			ran.header.stamp = rospy.Time.now()
			ran.header.frame_id = "/ultrasound"
			ran.field_of_view = 1;
			ran.min_range = 0;
			ran.max_range = 5;
			ran.range = 0.5;
			ultrasound_pub.publish(ran)
			rate.sleep()

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
 def altitude_pub(self, alt):
     rng = Range()
     rng.field_of_view = math.pi * 0.1
     rng.max_range = 300
     rng.header.frame_id = "sonar_link"
     rng.header.stamp = rospy.Time.now()
     rng.range = alt
     return rng

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
	def send_range(self, msg, current_time):
		# ultra sonic range finder
		scan = Range()
		scan.header.stamp = current_time
		scan.header.frame_id = "forward_sensor"
		scan.radiation_type = 0
		scan.field_of_view = 60*pi/180
		scan.min_range = 0.0
		scan.max_range = 4.0
		scan.range = msg.d1/100.0
		self.pub_sonar.publish(scan)

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
def sendMessage(sonarId, distance):
	sonarDictionary = {'1':"frontLeftSonar", '2':"diagLeftSonar", '3':"sideLeftSonar", '4':"frontRightSonar", '5':"diagRightSonar", '6':"sideRightSonar"}
	sonarName = sonarDictionary[sonarId]
	sonar = Range()
	sonar.header.stamp = rospy.Time.now()
	sonar.header.frame_id = sonarName
	sonar.range = float(distance)
	sonar.field_of_view = .3489
	sonar.max_range = 6.45
	sonar.min_range = .1524
	return sonar

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
    def _msg(self, frame_id, distance):
        msg = Range()
        msg.header.frame_id = frame_id
        msg.header.stamp = rospy.Time.now()
        msg.radiation_type = self._type
        msg.field_of_view = self._fov
        msg.min_range = self._min
        msg.max_range = self._max
        msg.range = min(max(distance, msg.min_range), msg.max_range)

        return msg

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
def sonar_publisher():
    # NAO connect
    # Connect to ALSonar module.
    sonarProxy = ALProxy("ALSonar", IP, PORT)
    # Subscribe to sonars, this will launch sonars (at hardware level) and start data acquisition.
    sonarProxy.subscribe("ROS_sonar_publisher")
    #Now you can retrieve sonar data from ALMemory.
    memoryProxy = ALProxy("ALMemory", IP, PORT)

    #ROS
    pub_right = rospy.Publisher(TOPIC_NAME+'right', Range, queue_size=10)
    pub_left = rospy.Publisher(TOPIC_NAME+'left', Range, queue_size=10)
    rospy.init_node('nao_sonar_publisher')
    r = rospy.Rate(PUBLISH_RATE)
    while not rospy.is_shutdown():
        # Get sonar left first echo (distance in meters to the first obstacle).
        left_range = memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
        # Same thing for right.
        right_range = memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")

        left = Range()
        left.header.stamp = rospy.Time.now()
        left.header.frame_id = '/torso'
        left.radiation_type = Range.ULTRASOUND
        left.field_of_view = 60
        left.min_range = 0.25 # m
        left.max_range = 2.55 # m
        left.range = left_range

        right = Range()
        right.header.stamp = rospy.Time.now()
        right.header.frame_id = '/torso'
        right.radiation_type = Range.ULTRASOUND
        right.field_of_view = 60
        right.min_range = 0.25 # m
        right.max_range = 2.55 # m
        right.range = right_range

        pub_right.publish(right)
        pub_left.publish(left)
        r.sleep()

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
    def default(self, ci="unused"):
        msg = Range()
        msg.radiation_type = Range.INFRARED
        msg.field_of_view = 20
        msg.min_range = 0
        msg.max_range = self.component_instance.bge_object["laser_range"]
        tmp = msg.max_range
        for ray in self.data["range_list"]:
            if tmp > ray:
                tmp = ray
        msg.range = tmp

        self.publish(msg)

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import range [as 别名]
 def publish(self):
     ts = rospy.Time.now()
     self.tf_broadcaster.sendTransform(self.position, self.orientation,
                                       ts, self.frame_id, 'odom')
     msg = Range()
     msg.header.stamp = ts
     msg.header.frame_id = self.frame_id
     msg.field_of_view = self.fov
     msg.max_range = self.max_range
     if self.caching and self.cache:
         msg.range = self.cache
     else:
         try:
             distance = self.nearest_point().distances[0]
             if distance > self.max_range:
                 msg.range = self.max_range
             else:
                 msg.range = distance
         except rospy.ServiceException:
             return
         self.cache = msg.range
     self.range_pub.publish(msg)