Python Range.field_of_view方法代码示例

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import field_of_view [as 别名]
 def publish(self, data):
     msg = Range()
     msg.header.stamp = rospy.get_rostime()
     msg.header.frame_id = self._frameId
     if self._urfType == URF_HRLV:
         msg.min_range = MIN_RANGE_URF_HRLV_MaxSonar
         msg.max_range = MAX_RANGE_URF_HRLV_MaxSonar
         msg.field_of_view = FIELD_OF_VIEW_URF_HRLV_MaxSonar
     else:
         msg.min_range = MIN_RANGE_URF_LV_MaxSonar
         msg.max_range = MAX_RANGE_URF_LV_MaxSonar
         msg.field_of_view = FIELD_OF_VIEW_URF_LV_MaxSonar
     msg.radiation_type = Range.ULTRASOUND
     msg.range = data
     self._pub.publish(msg)

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import field_of_view [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 field_of_view [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 field_of_view [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 field_of_view [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 field_of_view [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 field_of_view [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 field_of_view [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 field_of_view [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 field_of_view [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 field_of_view [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 field_of_view [as 别名]
    def default(self, ci='unused'):
        """ Publish the data of the infrared sensor as a ROS Range message """
        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 field_of_view [as 别名]
    def default(self, ci='unused'):
        msg = Range()
        msg.header = self.get_ros_header()
        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 field_of_view [as 别名]
 def inputCallback(self, msg):
 #########################################################################
     # rospy.loginfo("-D- range_filter inputCallback")
     cur_val = msg.value
 
     if cur_val <= self.max_valid and cur_val >= self.min_valid:
         self.prev.append(cur_val)
         del self.prev[0]
     
         p = array(self.prev)
         self.rolling_ave = p.mean()
         self.rolling_std = p.std()
     
         self.rolling_meters = ((self.b * self.rolling_ave) ** self.m) / 100
     
         self.filtered_pub.publish( self.rolling_meters )
         self.std_pub.publish( self.rolling_std )
         
         rng = Range()
         rng.radiation_type = 1
         rng.min_range = self.min_range
         rng.max_range = self.max_range
         rng.range = self.rolling_meters
         rng.header.frame_id = self.frame
         rng.field_of_view = 0.1
         rng.header.stamp = rospy.Time.now()
         
         self.range_pub.publish( rng )
        
         ranges = [] 
         intensities = []
         angle_start = 0.0 - self.field_of_view
         angle_stop = self.field_of_view
         for angle in linspace( angle_start, angle_stop, 10):
             ranges.append( self.rolling_meters )
             intensities.append( 1.0 )
         scan = LaserScan()
         scan.ranges = ranges
         scan.header.frame_id = self.frame
         scan.time_increment = 0;
         scan.range_min = self.min_range
         scan.range_max = self.max_range
         scan.angle_min = angle_start
         scan.angle_max = angle_stop
         scan.angle_increment = (angle_stop - angle_start) / 10
         scan.intensities = intensities
         scan.scan_time = self.scan_time
         scan.header.stamp = rospy.Time.now()
         self.scan_pub.publish( scan )

# 需要导入模块: from sensor_msgs.msg import Range [as 别名]
# 或者: from sensor_msgs.msg.Range import field_of_view [as 别名]
    def getDistance(self):
		while True:
			self.dist=input("plase input dis:") 
			try:
				print "you have input:"+str(self.dist)
			except:
				print "you have input nothing"
			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;
			self.ultrasound_pub.publish(ran)