def _hw_ultrasonics_cb(self, msg): leftMsg = Range() leftMsg.header.stamp = rospy.Time.now() leftMsg.header.frame_id = "left_ultrasonic_link" leftMsg.radiation_type = Range.ULTRASOUND leftMsg.field_of_view = msg.field_of_view leftMsg.min_range = msg.min_range leftMsg.max_range = msg.max_range leftMsg.range = msg.left_range self._left_ultra_pub.publish(leftMsg) centerMsg = Range() centerMsg.header.stamp = rospy.Time.now() centerMsg.header.frame_id = "center_ultrasonic_link" centerMsg.radiation_type = Range.ULTRASOUND centerMsg.field_of_view = msg.field_of_view centerMsg.min_range = msg.min_range centerMsg.max_range = msg.max_range centerMsg.range = msg.center_range self._center_ultra_pub.publish(centerMsg) rightMsg = Range() rightMsg.header.stamp = rospy.Time.now() rightMsg.header.frame_id = "right_ultrasonic_link" rightMsg.radiation_type = Range.ULTRASOUND rightMsg.field_of_view = msg.field_of_view rightMsg.min_range = msg.min_range rightMsg.max_range = msg.max_range rightMsg.range = msg.right_range self._right_ultra_pub.publish(rightMsg)
def talker(): top_pub = rospy.Publisher('/IR_top', Range, queue_size=1) left_pub = rospy.Publisher('/IR_left', Range, queue_size=1) right_pub = rospy.Publisher('/IR_right', Range, queue_size=1) rospy.init_node('IR_node', anonymous=True) rate = rospy.Rate(6) # 10hz #top_data top_range = Range() top_range.header.frame_id = '/IR_top' top_range.field_of_view = 0.4 top_range.max_range = 3 top_range.min_range = 0.003 print('TOF start') #left left_range = Range() left_range.header.frame_id = '/IR_left' left_range.field_of_view = 0.4 left_range.max_range = 3 left_range.min_range = 0.005 #right right_range = Range() right_range.header.frame_id = '/IR_right' right_range.field_of_view = 0.4 right_range.max_range = 3 right_range.min_range = 0.003 time.sleep(3) while not rospy.is_shutdown(): receiced_distance = ir_pub() #top top_range.header.stamp = rospy.Time.now() top_range.range = receiced_distance[0] #print('top is %f \n' % top_range.range) top_pub.publish(top_range) #left left_range.header.stamp = rospy.Time.now() left_range.range = receiced_distance[12] # print('left is %f \n' %left_range.range) left_pub.publish(left_range) #right right_range.header.stamp = rospy.Time.now() right_range.range = receiced_distance[4] #print('right is %f \n' % right_range.range) right_pub.publish(right_range) rate.sleep()
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)
def main(): GPG = gopigo3.GoPiGo3() rospy.init_node("ultra_sonic") port_id = rospy.get_param("~port", 1) if port_id == 1: port = GPG.GROVE_1 elif port_id == 2: port = GPG.GROVE_2 else: rospy.logerr("unknown port %i", port_id) return GPG.set_grove_type(port, GPG.GROVE_TYPE.US) pub_distance = rospy.Publisher("~distance", Range, queue_size=10) msg_range = Range() msg_range.header.frame_id = "ultra_sonic" msg_range.radiation_type = Range.ULTRASOUND msg_range.min_range = 0.02 msg_range.max_range = 4.3 rate = rospy.Rate(rospy.get_param('~hz', 30)) while not rospy.is_shutdown(): # read distance in meter msg_range.range = GPG.get_grove_value(port) / 1000.0 msg_range.header.stamp = rospy.Time.now() pub_distance.publish(msg_range) rate.sleep()
def main(): rospy.init_node('range_faker', anonymous=True) rate = rospy.Rate(1) # 1Hz frames_in = rospy.get_param('~frames_in') topic_out = rospy.get_param('~topic_out') pub = rospy.Publisher(topic_out, Range, queue_size=10) while not rospy.is_shutdown(): t = rospy.Time.now() for frame in frames_in: msg = Range() # defaults msg.radiation_type = Range.INFRARED msg.field_of_view = 1 msg.min_range = 0.1 msg.max_range = 0.8 msg.header.stamp = t # custom msg.header.frame_id = frame msg.range = 0.7 pub.publish(msg) rate.sleep() pub.unregister()
def test_map_received(self): poseMsg = PoseStamped() poseMsg.header = Header() poseMsg.pose.position.x = 0 poseMsg.pose.position.y = 0 poseMsg.pose.position.z = 0 poseMsg.pose.orientation.x = 0 poseMsg.pose.orientation.y = 0 poseMsg.pose.orientation.z = 0 poseMsg.pose.orientation.w = 1 self.pose_pub.publish(poseMsg) rangeMsg = RangeArray() rangeMsg.header = Header() ranges = [0, 0.2, -1, 1, 1.5, 8, 3, 1, 8, 4, 2.3, 0.2, 0.1, 2, 3, 1, 9] for rangerange in ranges: range = Range() range.header = Header() range.range = rangerange range.field_of_view = 1 range.min_range = 0.1 range.max_range = 2.1 rangeMsg.ranges.append(range) rangeMsg.ranges = rangeMsg.ranges[:self.NUM_RANGE_SENSORS] self.range_pub.publish(rangeMsg) rospy.sleep(0.2) self.assertEqual(self.rx.receivedMsg, True) self.assertGreater(len(self.rx.map.points), 0)
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)
def camera_callback(self): image = [] self.camera.enable(self.timestep) self.frontDistance_sensor.enable(self.timestep) self.camera.recognitionEnable(self.timestep) msg_camera = Range() if(len(self.camera.getRecognitionObjects()) > 0): object = self.camera.getRecognitionObjects()[0] position = (abs(object.get_position()[0])) color = object.get_colors() self.get_logger().info(str(color)) id = object.get_id() pos_img = object.get_position_on_image() msg_camera.radiation_type = id msg_camera.range = position if((abs(position < 0.02))): if(self.object1 == 0.0 and self.object2 == 0.0 and self.object3 == 0.0): self.object1 = self.frontDistance_sensor.getValue() self.id1 = id if(self.object1 != 0.0 and self.object2 == 0.0 and self.object3 == 0.0 and self.id1 != id): self.object2 = self.frontDistance_sensor.getValue() self.id2 = id if(self.object1 != 0.0 and self.object2 != 0.0 and self.object3 == 0.0 and self.id2 != id): self.object3 = self.frontDistance_sensor.getValue() self.id3 = id if(self.object1 > 0.0 and self.object2 > 0.0 and self.object3 > 0.0 and self.count == 0): msg_camera.field_of_view = self.object1 msg_camera.min_range = self.object2 msg_camera.max_range = self.object3 self.camera_publisher.publish(msg_camera) self.get_logger().info(str(msg_camera)) self.count = 1
def main(): # instantiate the distance object #sensor = DistanceSensor() my_sensor = EasyDistanceSensor() #sensor.start_continuous() rospy.init_node("distance_sensor") pub_distance = rospy.Publisher("~distance", Range, queue_size=10) msg_range = Range() msg_range.header.frame_id = "distance" msg_range.radiation_type = Range.INFRARED msg_range.min_range = 0.02 msg_range.max_range = 3.0 rate = rospy.Rate(rospy.get_param('~hz', 1)) while not rospy.is_shutdown(): # read distance in meters read_distance = my_sensor.read() / 100.0 msg_range.range = read_distance # Add timestamp msg_range.header.stamp = rospy.Time.now() # Print current distance print msg_range.range * 1000, " mm" # Publish distance message pub_distance.publish(msg_range) rate.sleep()
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())
def callback(self, msg): ranges = msg.ranges terarangers_msg = Range() terarangers_msg.header.frame_id = "base_range" terarangers_msg.radiation_type = 1 terarangers_msg.field_of_view = 0.0593 terarangers_msg.min_range = 200 terarangers_msg.max_range = 14000 # 14 metres v0 = ranges[45] * 1000 #45 degree and convert m to mm v1 = ranges[135] * 1000 #135 degree and convert m to mm v2 = ranges[225] * 1000 #225 degree and convert m to mm v3 = ranges[315] * 1000 #315 degree and convert m to mm terarangers_msg.header.stamp = rospy.Time.now() terarangers_msg.range = v0 self.trone0_pub.publish(terarangers_msg) terarangers_msg.header.stamp = rospy.Time.now() terarangers_msg.range = v1 self.trone1_pub.publish(terarangers_msg) terarangers_msg.header.stamp = rospy.Time.now() terarangers_msg.range = v2 self.trone2_pub.publish(terarangers_msg) terarangers_msg.header.stamp = rospy.Time.now() terarangers_msg.range = v3 self.trone3_pub.publish(terarangers_msg)
def timerCallback(self, event): data = self.pinger.get_distance() if data: range_msg = Range() range_msg.header.stamp = rospy.Time.now() range_msg.header.frame_id = 'pinger' range_msg.header.seq = self.count range_msg.field_of_view = self.field_of_view range_msg.min_range = 0.5 range_msg.max_range = 30.0 range_msg.range = float(data["distance"]) / 1000.0 self.range_pub.publish(range_msg) ping_msg = PingRange() ping_msg.header = range_msg.header ping_msg.field_of_view = range_msg.field_of_view ping_msg.min_range = range_msg.min_range ping_msg.max_range = range_msg.max_range ping_msg.range = range_msg.range ping_msg.confidence = data['confidence'] / 100.0 self.ping_pub.publish(ping_msg) else: oar_msg = Bool() oar_msg.data = True self.out_of_range_pub.publish(oar_msg) rospy.loginfo("Failed to get distance data") self.count += 1
def __init__(self): rospy.init_node('sonar', anonymous=True) topic_name_d = '/sonarTP_D' self.distance_publisher_down = rospy.Publisher(topic_name_d, Range, queue_size=5) self.r = rospy.Rate(40) self.serialPort0 = "/dev/ttyUSB0" self.serialPort1 = "/dev/ttyUSB0" self.serialPort2 = "/dev/ttyUSB0" self.serialPort3 = "/dev/ttyUSB0" self.serialPort4 = "/dev/ttyUSB0" self.maxRange = 2000 # change for 5m vs 10m sensor self.sleepTime = 0.1 self.minMM = 0.3 self.maxMM = 3 self.mm = float r = Range() r.header.stamp = rospy.Time.now() r.header.frame_id = "/sonarD_link" r.radiation_type = 0 r.field_of_view = 0.8 self.min_range = self.minMM self.max_range = self.maxRange r.min_range = self.min_range r.max_range = self.max_range self._range = r
def _publish_laser(self): """ TODO Griffin: Add a transform from base_link to base_laser TODO: Modify the is valid condition if significant slope traversal object detection required. Publish filtered laser distance as Range message. Will publish NaN distance if scanner is blocked or robot in orientation not suited to horizontal navigation. See API for sensor range, fov etc https://developer.anki.com/vector/docs/generated/anki_vector.proximity.html """ if self._laser_pub.get_num_connections() == 0: return now = rospy.Time.now() laser = Range() laser.header.frame_id = self._base_frame laser.header.stamp = now laser.radiation_type = 1 # IR laser laser.field_of_view = 0.436332 # 25 deg laser.min_range = 0.03 # 30mm laser.max_range = 1.5 # 300mm laser_reading = self._vector.proximity.last_sensor_reading if(laser_reading.is_valid): laser.range = self._vector.proximity.last_sensor_reading.distance.distance_mm/1000 else: laser.range = float('nan') self._laser_pub.publish(laser)
def main(): rospy.init_node('dwm1001_driver_node') rospy.loginfo('Starting the dwm1001 driver node') main_class = MainClass() main_class.read_parameters() ser = serial.Serial(main_class.serial_port_, 115200, timeout=1) rate = rospy.Rate(200) # 1000Hz range_msg = Range() range_msg.min_range = 0.0 range_msg.max_range = 200.0 while not rospy.is_shutdown(): line = ser.readline() data = {} try: data = json.loads(line) except ValueError as e: continue range_msg.header.stamp = rospy.get_rostime() if 'nrng' not in data: continue if 'rng' not in data['nrng']: continue if 'uid' not in data['nrng']: continue for i in range(len(data['nrng']['uid'])): range_msg.range = float(data['nrng']['rng'][i]) main_class.publishers_[int( data['nrng']['uid'][i])].publish(range_msg) rate.sleep()
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()
def publish_sonar(): rospy.init_node('sonar_publisher', anonymous=True) sonar_topic = rospy.get_param('~sonar_topic') pub = rospy.Publisher(sonar_topic, Range, queue_size=1) #initialize sonars define_sonars() init_sonars() #rospy.init_node('sonar_publisher', anonymous=True) rate = rospy.Rate(10) # 10hz current_altitude = Range() while not rospy.is_shutdown(): current_altitude.header.seq += 1 current_altitude.header.stamp = rospy.get_rostime() current_altitude.header.frame_id = "sonar_frame" current_altitude.min_range = 0.02 current_altitude.max_range = 3.5 sonar1 = read_sonar(1) #time.sleep(0.02) #if sonar1 >= 3.5 or sonar2 > 3.5: #current_altitude.range = 3.5 current_altitude.range = sonar1 pub.publish(current_altitude) rate.sleep()
def distance_thread(): global b # ROS topic publisher for sonar distance readings pub = rospy.Publisher('nxt/sonar', Range, queue_size=10) # Sonar driver ultrasonic = Ultrasonic(b, PORT_4) # Setup the ROS range message with sensor properties range_msg = Range() range_msg.radiation_type = Range.ULTRASOUND range_msg.field_of_view = 10.0 * pi / 180.0 range_msg.min_range = 0.01 range_msg.max_range = 2.55 # Rate limiter for process loop rate = rospy.Rate(4) # Main process loop while not rospy.is_shutdown(): # Perform and read US range measurement try: d = ultrasonic.get_sample() except: rospy.logfatal("Connection with NXT robot interrupted!") rospy.signal_shutdown("Connexion error") # Fill and publish the ROS range message range_msg.header.stamp = rospy.Time.now() range_msg.range = 0.01 * d pub.publish(range_msg) # Wait until next loop rate.sleep()
def main(): range_data = Range() rospy.init_node('srf_08_rangefinder', anonymous=True) device_address = rospy.get_param('~device_addr') sample_rate = rospy.get_param('~sample_rate') range_data.radiation_type = rospy.get_param('~radiation_type') range_data.min_range = rospy.get_param('~min_range') range_data.max_range = rospy.get_param('~max_range') range_data.field_of_view = rospy.get_param('~fov') pub = rospy.Publisher('rangefinder_data', Range, queue_size=1) rate = rospy.Rate(sample_rate) rangefinder = srf_08(device_address) while not rospy.is_shutdown(): range_data.header.stamp = rospy.Time.now() range_data.range = (rangefinder.distance() / 100) pub.publish(range_data) rate.sleep()
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()
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()
def __init__( self, num_sensor=5, #Number of active infrared sensors range_min=1.0, #Minimum valid range of sensor [m] range_max=4.0 #Maximum valid range of sensor [m] ): super().__init__("ir_data") self.ir_array = [] self.pub_array = [] self.distance = [0, 0, 0, 0, 0] self.num_sensor = num_sensor timer_period = 0.05 self.timer = self.create_timer(timer_period, self.scan) for i in range(num_sensor): ir_num = i + 1 ir = IR() ir.angle = math.radians(5) self.ir_array.append(ir) topic_name = "/agv/ir_%d" % ir_num topic_name = str(topic_name) pub = self.create_publisher(Range, topic_name, 5) self.pub_array.append(pub) #Default message message = Range() message.radiation_type = 0 message.min_range = range_min message.max_range = range_max self.msg = message
def __init__( self, num_sensor=5, #Number of active ultrasonic sensors range_min=0.04, #Minimum valid range of sensor [m] range_max=3.5 #Maximum valid range of sensor [m] ): super().__init__("sonar_data") self.sonar_array = [] self.pub_array = [] self.distance = [0, 0, 0, 0, 0] self.num_sensor = num_sensor timer_period = 0.05 self.timer = self.create_timer(timer_period, self.scan) for i in range(num_sensor): sonar_num = i + 1 sonar = Sonar() sonar.angle = math.radians(15) self.sonar_array.append(sonar) topic_name = "/agv/sonar_%d" % sonar_num topic_name = str(topic_name) pub = self.create_publisher(Range, topic_name, 5) self.pub_array.append(pub) sonar_num = 0 #Default message message = Range() message.radiation_type = 0 message.min_range = range_min message.max_range = range_max self.msg = message
def distance_callback(self, stamp): distance_from_center = 0.035 for key in self.sensors: msg = Range() msg.field_of_view = self.sensors[key].getAperture() msg.min_range = intensity_to_distance( self.sensors[key].getMaxValue() - 8.2) + distance_from_center msg.max_range = intensity_to_distance( self.sensors[key].getMinValue() + 3.3) + distance_from_center msg.range = intensity_to_distance(self.sensors[key].getValue()) msg.radiation_type = Range.INFRARED self.sensor_publishers[key].publish(msg) # Max range of ToF sensor is 2m so we put it as maximum laser range. # Therefore, for all invalid ranges we put 0 so it get deleted by rviz msg = LaserScan() msg.header.frame_id = 'laser_scanner' msg.header.stamp = stamp msg.angle_min = 0.0 msg.angle_max = 2 * pi msg.angle_increment = 15 * pi / 180.0 msg.scan_time = self.timestep.value / 1000 msg.range_min = intensity_to_distance( self.sensors['ps0'].getMaxValue() - 20) + distance_from_center msg.range_max = 1.0 + distance_from_center msg.ranges = [ self.sensors['tof'].getValue(), # 0 intensity_to_distance(self.sensors['ps7'].getValue()), # 15 0.0, # 30 intensity_to_distance(self.sensors['ps6'].getValue()), # 45 0.0, # 60 0.0, # 75 intensity_to_distance(self.sensors['ps5'].getValue()), # 90 0.0, # 105 0.0, # 120 0.0, # 135 intensity_to_distance(self.sensors['ps4'].getValue()), # 150 0.0, # 165 0.0, # 180 0.0, # 195 intensity_to_distance(self.sensors['ps3'].getValue()), # 210 0.0, # 225 0.0, # 240 0.0, # 255 intensity_to_distance(self.sensors['ps2'].getValue()), # 270 0.0, # 285 0.0, # 300 intensity_to_distance(self.sensors['ps1'].getValue()), # 315 0.0, # 330 intensity_to_distance(self.sensors['ps0'].getValue()), # 345 self.sensors['tof'].getValue(), # 0 ] for i in range(len(msg.ranges)): if msg.ranges[i] != 0: msg.ranges[i] += distance_from_center self.laser_publisher.publish(msg)
def prepareArray(self): for i in range(0, len(self.sensors)): r = Range() r.header.frame_id = "ir{}_link".format(i + 1) r.min_range = self.sensors[i].min_range r.max_range = self.sensors[i].max_range r.radiation_type = r.INFRARED self.msg.array += [r]
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
def out_message(self): msg = Range() msg.max_range = self.max_range msg.min_range = self.min_range msg.range = self.return_median() msg.header = self.header self.publisher.publish(msg) print "Last four ranges recived: ", np.around(self.ranges, 4), "\t\t\t\r"
def trigger(self): ds = Range() ds.header.frame_id = '/ultrasonic_link' ds.header.stamp = rospy.Time.now() ds.range = self.ultrasonic.get_sample() / 100.0 ds.field_of_view = self.spread ds.min_range = self.min_range ds.max_range = self.max_range self.pub.publish(ds)
def pub_depth(self): msg = Range() msg.radiation_type = 0 msg.min_range = 0.5 msg.max_range = 30.0 msg.range = random.uniform(1.0, 5.0) msg.field_of_view = 100.0 msg.header.stamp = self.get_clock().now().to_msg() self.p_depth.publish(msg)
def onLidarLiteUpdate(self, msg): inMsg = msg outMsg = Range() outMsg.header = inMsg.header outMsg.min_range = inMsg.range_min outMsg.max_range = inMsg.range_max outMsg.range = msg.ranges[1] self.LidarLitePub.publish(outMsg)
def init(): global msg, device_range, range_pub msg = Range() msg.radiation_type = 2 msg.field_of_view = 3.1415 msg.min_range = 0.2 msg.max_range = 10.0 # depends on the configuration, 300m on datasheet, 10m reported by users device_range = pypozyx.DeviceRange() rospy.init_node('pozyx_pose_node')
def publish_range(self, range): """Create and publish the Range message to publisher.""" msg = Range() msg.max_range = 0.8 msg.min_range = 0 msg.range = range msg.header.frame_id = "base" msg.header.stamp = rospy.Time.now() self.range_pub.publish(msg)
def trigger(self): ds = Range() ds.header.frame_id = '/ultrasonic_link' ds.header.stamp = rospy.Time.now() ds.range = self.ultrasonic.get_sample()/100.0 ds.field_of_view = self.spread ds.min_range = self.min_range ds.max_range = self.max_range self.pub.publish(ds)
def publish_range(self, input_range): """Create and publish the Range message to publisher.""" msg = Range() msg.max_range = self.max_range #different max for lidar version msg.min_range = 0.0 #range in meters msg.range = input_range msg.header.frame_id = "base" msg.header.stamp = rospy.Time.now() self.range_pub.publish(msg)
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
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)
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
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()
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)
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
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)
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)
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 )
def talker(): pub = rospy.Publisher("sensor1", Range, queue_size=50) rospy.init_node("robot", anonymous=True) rate = rospy.Rate(1) # 10hz while not rospy.is_shutdown(): msg = Range() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "/sensor1" msg.min_range = 0 msg.max_range = 2 msg.radiation_type = Range.ULTRASOUND rospy.loginfo(msg) pub.publish(msg) rate.sleep()
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)
def sendLidar(): global lasers_raw lidar_publisher = rospy.Publisher('mavros/distance_sensor/lidar', Range, queue_size=1) rate = rospy.Rate(30) msg = Range() sendLidar_count = 0 msg.radiation_type = msg.INFRARED msg.field_of_view = 0.0523599 # 3° in radians msg.min_range = 0.05 msg.max_range = 20.0 while run: msg.header.stamp = rospy.Time.now() msg.header.seq=sendLidar_count msg.range = (lasers_raw.lasers[4] + lasers_raw.lasers[5])/200 lidar_publisher.publish(msg) sendLidar_count = sendLidar_count + 1 rate.sleep()
def callback(event): value = ADC.read("P9_39") outvoltage = 3.3 * value #minimum range if outvoltage > 2.75: rangevalue = 15 elif outvoltage < .4: rangevalue = 150 else: #exponential fit function, derived from data sheet rangevalue = 61.7 * math.pow(outvoltage, -1.2) #assign values and publish pub = rospy.Publisher('/range', Range, queue_size=10) rangeout = Range() rangeout.min_range = 15 rangeout.max_range = 150 rangeout.range = rangevalue rospy.loginfo(rangeout) pub.publish(rangeout)
def main(): pub = rospy.Publisher('ernest_sensors/imu', Imu, queue_size=10) eyes = rospy.Publisher('ernest_sensors/eyes', Range, queue_size=10) nunchuck = rospy.Publisher('ernest_sensors/joy', Joy, queue_size=10) nunchuck_giro = rospy.Publisher('ernest_sensors/joy_giro', Imu, queue_size=10) comm = ArduinoCommunicator() rospy.init_node('ernest_sensors') for line in comm.loop(): print line try: x, y, z, dis, joy_x, joy_y, giro_x, giro_y, giro_z, but_c, but_z = line.split(",") imu = Imu() imu.header.frame_id = "imu" imu.linear_acceleration.x = -float(x) imu.linear_acceleration.y = float(z) imu.linear_acceleration.z = float(y) r = Range() r.header.frame_id = "imu" r.radiation_type = 1 r.field_of_view = 0.5 r.min_range = 0.20 r.max_range = 3 r.range = float(dis)/100.0 j = Joy() j.axes = [ maprange(float(joy_x), 25, 226, -1, 1), maprange(float(joy_y), 32, 223, -1, 1) ] j.buttons = [int(but_c), int(but_z)] imu2 = Imu() imu2.header.frame_id = "imu" imu2.linear_acceleration.x = (float(giro_y) - 512) / 512.0 imu2.linear_acceleration.y = -(float(giro_x) - 512) / 512.0 imu2.linear_acceleration.z = -(float(giro_z) - 512) / 512.0 pub.publish(imu) eyes.publish(r) nunchuck.publish(j) nunchuck_giro.publish(imu2) except ValueError: continue
def lightware_ros(): rospy.init_node('lightware_ros') # ROS parameters hz = rospy.get_param('~rate', -1) port = rospy.get_param('~port', '/dev/ttyUSB0') t_out = rospy.get_param('~timeout', 0.05) baudrate = rospy.get_param('~baudrate', 115200) # init ROS stuff laser_pub = rospy.Publisher('range', Range, queue_size=10) if hz > 0: rate = rospy.Rate(hz) with serial.Serial(port, baudrate, timeout=t_out) as ser: while ser.isOpen() and not rospy.is_shutdown(): ser.reset_input_buffer() payload = ser.readline() range_string = payload.split("m")[0] try: distance = float(range_string) # populate message msg = Range() msg.header.stamp = rospy.Time.now() msg.radiation_type = 1 msg.field_of_view = 0.0035 # rad msg.min_range = 0.0 # m msg.max_range = 50.0 # m msg.range = distance laser_pub.publish(msg) except ValueError: rospy.logwarn('Serial Read Error: %s', payload) if hz > 0: rate.sleep()
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)
def post_range(self, component_instance): """ Publish the data on the rostopic http://www.ros.org/doc/api/sensor_msgs/html/msg/Range.html """ msg = Range() #msg.header.frame_id = 'infrared' msg.radiation_type = Range.INFRARED msg.field_of_view = 20 msg.min_range = 0 msg.max_range = component_instance.blender_obj['laser_range'] tmp = component_instance.blender_obj['laser_range'] for r in component_instance.local_data['range_list']: if tmp > r: tmp = r msg.range = tmp parent_name = component_instance.robot_parent.blender_obj.name for topic in self._topics: # publish the message on the correct topic if str(topic.name) == str("/" + parent_name + "/" + component_instance.blender_obj.name): topic.publish(msg)
def talker(): signal.signal(signal.SIGINT, signal_handler) time.sleep(1) pub = rospy.Publisher('ultrasonic_data', Range, queue_size=10) rospy.init_node('ultrasonic_node', anonymous=True, disable_signals=False ) rate = rospy.Rate(10) # 10hz ultrasonic_number = 1 while not rospy.is_shutdown(): data = Range() data.header.frame_id = ""+str(ultrasonic_number) data.header.stamp = rospy.Time.now() data.radiation_type = 0 # ULTRASONIC data.min_range = 5.0 data.max_range = 200.0 try : data.range = get_us_data_from(ultrasonic_number) except IOError: subprocess.call(['i2cdetect', '-y', '1']) data.range = 0 #rospy.loginfo() pub.publish(data) ultrasonic_number += 1 if ultrasonic_number == 6: ultrasonic_number = 1
def talker(): #XBee config ser = serial.Serial("/dev/ttyUSB0", 9600) ser.write('\n') ser.write('b\n') ser.close() ser = serial.Serial("/dev/ttyUSB0", 115200) sourceaddrlist = {} # node set dictionary pubMarker = {} # publisher dictionary pubText = {} # publisher dictionary pubUS1 = {} pubUS2 = {} pubUS3 = {} pubUS4 = {} xbradio1 = xbee.ZigBee(ser) # the DAQ radio rospy.init_node('RelocNode') pubIRdata1=rospy.Publisher('/IRdebug/point1', Point) pubIRdata2=rospy.Publisher('/IRdebug/point2', Point) pubIRdata3=rospy.Publisher('/IRdebug/point3', Point) pubIRdata4=rospy.Publisher('/IRdebug/point4', Point) pubUSdata=rospy.Publisher('/USdebug', Quaternion) br = tf.TransformBroadcaster() robId="??" IRx1=0 IRy1=0 IRx2=0 IRy2=0 IRx3=0 IRy3=0 IRx4=0 IRy4=0 IRang=0 US1=0 US2=0 US3=0 US4=0 time_pre=0 while not rospy.is_shutdown(): response = xbradio1.wait_read_frame() #response is a python dictionary #sorce address name=binascii.hexlify(response['source_addr_long']) sourceaddr=name.decode("utf-8") # check if its a new source address and initialize publishers nodeseen = 0 for j in range(len(sourceaddrlist)): if sourceaddrlist[j+1]==sourceaddr: nodeseen = 1 Rfdata=response['rf_data'].decode("utf-8") str2=Rfdata.split(',') #packet data extraction range_flag =0 bearing_flag =0 packetValid=0 try: for i in range(len(str2)): if str2[i][0:2] == "RL": robID = str2[i][2:] if str2[i] == "IR": IRx1=int(str2[i+1]) IRy1=int(str2[i+2]) IRx2=int(str2[i+3]) IRy2=int(str2[i+4]) IRx3=int(str2[i+5]) IRy3=int(str2[i+6]) IRx4=int(str2[i+7]) IRy4=int(str2[i+8]) bearing_flag =1 if str2[i] == "ANG": IRang=int(str2[i+1][0:len(str2[i+1])-2])-90 #print IRang/180.0*math.pi br.sendTransform((0, 0, 0.3),tf.transformations.quaternion_from_euler(0, 0, -IRang/180.0*math.pi),rospy.Time.now(),"/pioneer1/IRcam","/pioneer1/base_reloc1") #br.sendTransform((0, 0, 0.3),tf.transformations.quaternion_from_euler(0, 0, -IRang/200.0*2*math.pi),rospy.Time.now(),"/pioneer1/IRcam","/pioneer1/base_reloc1") if str2[i] == "US": US1=float(str2[i+1]) US2=float(str2[i+2]) US3=float(str2[i+3]) US4=float(str2[i+4]) range_flag =1 packetValid=1 except: #print "Bad Packet\n" packetValid=0 #Measurement processing if packetValid==1: slog = "No Data" USmin=0 if range_flag==1 and bearing_flag==0: IRx=0 IRy=0 USmin=min(US1,US2,US3,US4)/1000.0 fc =1380 slog = "Range only" if USmin==10725: slog = "No Data" USmin=0 if range_flag==0 and bearing_flag==1: IRx=IRx1-1024/2 IRy=-IRy1+768/2 USmin=10 fc =1380 slog = "Bearing only" if range_flag==1 and bearing_flag==1: IRx=IRx1-1024/2 IRy=-IRy1+768/2 USmin=min(US1,US2,US3,US4)/1000.0 fc =1380 slog = "Range Bearing" #print USmin #print IRx1 if IRx1==1023: robx=0 roby=0 robz=0 else: rpx=math.sqrt(pow(IRx,2)+pow(IRy,2)+pow(fc,2)) robx=fc/rpx*USmin roby=IRx/rpx*USmin robz=IRy/rpx*USmin if USmin==0 or USmin>9: USmin=0.1 #Data publish text_marker = Marker() text_marker.header.frame_id="/pioneer1/base_link" text_marker.type = Marker.TEXT_VIEW_FACING text_marker.text = "RobID : %s\nx : %f\ny : %f\nz : %f\ntheta :\n %s" % (robID,robx,roby,robz,slog) text_marker.pose.position.x= robx text_marker.pose.position.y= roby text_marker.pose.position.z= robz text_marker.scale.z = 0.1 text_marker.color.r = 0.0 text_marker.color.g = 0.5 text_marker.color.b = 0.5 text_marker.color.a = 1 if slog == "Range only": arrow_marker = Marker() arrow_marker.header.frame_id="/pioneer1/base_reloc1" arrow_marker.type = Marker.CYLINDER arrow_marker.scale.x = (USmin+0.05)*2 arrow_marker.scale.y = (USmin+0.05)*2 arrow_marker.scale.z = 0.1 arrow_marker.color.r = 0.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 0.5 else : arrow_marker = Marker() arrow_marker.header.frame_id="/pioneer1/base_reloc1" #arrow_marker.header.frame_id="/pioneer1/IRcam" arrow_marker.type = Marker.ARROW arrow_marker.points = {Point(0,0,0),Point(robx,roby,robz)} arrow_marker.scale.x = 0.1 arrow_marker.scale.y = 0.1 arrow_marker.scale.z = 0.2 arrow_marker.color.r = 1.0 arrow_marker.color.g = 0.5 arrow_marker.color.b = 0.5 arrow_marker.color.a = 0.5 IRdebug1 = Point() IRdebug1.x = IRx1 IRdebug1.y = IRy1 IRdebug2 = Point() IRdebug2.x = IRx2 IRdebug2.y = IRy2 IRdebug3 = Point() IRdebug3.x = IRx3 IRdebug3.y = IRy3 IRdebug4 = Point() IRdebug4.x = IRx4 IRdebug4.y = IRy4 USdebug = Quaternion() USdebug.x = US1 USdebug.y = US2 USdebug.z = US3 USdebug.w = US4 USrange1 = Range() USrange1.header.frame_id="/US1" USrange1.radiation_type=0 USrange1.field_of_view =math.pi/2.0 USrange1.min_range = 0.5 USrange1.max_range = 10.0 USrange1.range = US3/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/US1","/pioneer2/base_reloc2") USrange2 = Range() USrange2.header.frame_id="/US2" USrange2.radiation_type=0 USrange2.field_of_view =math.pi/2.0 USrange2.min_range = 0.5 USrange2.max_range = 10.0 USrange2.range = US2/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, math.pi/2.0),rospy.Time.now(),"/US2","/US1") USrange3 = Range() USrange3.header.frame_id="/US3" USrange3.radiation_type=0 USrange3.field_of_view =math.pi/2.0 USrange3.min_range = 0.5 USrange3.max_range = 10.0 USrange3.range = US4/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, math.pi),rospy.Time.now(),"/US3","/US1") USrange4 = Range() USrange4.header.frame_id="/US4" USrange4.radiation_type=0 USrange4.field_of_view =math.pi/2.0 USrange4.min_range = 0.5 USrange4.max_range = 10.0 USrange4.range = US1/1000.0 br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, 3.0*math.pi/2.0),rospy.Time.now(),"/US4","/US1") pubIRdata1.publish(IRdebug1) pubIRdata2.publish(IRdebug2) pubIRdata3.publish(IRdebug3) pubIRdata4.publish(IRdebug4) pubUSdata.publish(USdebug) pubText[j+1].publish(text_marker) pubMarker[j+1].publish(arrow_marker) pubUS1[j+1].publish(USrange1) pubUS2[j+1].publish(USrange2) pubUS3[j+1].publish(USrange3) pubUS4[j+1].publish(USrange4) if str2[0][0:5] == "RL001": #time_now=time.time() print str2 #time_pre=time_now #print str2 if nodeseen == 0: # New nodes handling sourceaddrlist[len(sourceaddrlist)+1]=sourceaddr pubText[len(pubText)+1]=rospy.Publisher('/relocNode'+str(len(pubText)+1)+'/relocVizDataText', Marker) pubMarker[len(pubMarker)+1]=rospy.Publisher('/relocNode'+str(len(pubMarker)+1)+'/relocVizData', Marker) pubUS1[len(pubMarker)]=rospy.Publisher('/relocNode'+str(len(pubMarker))+'/relocUS1', Range) pubUS2[len(pubMarker)]=rospy.Publisher('/relocNode'+str(len(pubMarker))+'/relocUS2', Range) pubUS3[len(pubMarker)]=rospy.Publisher('/relocNode'+str(len(pubMarker))+'/relocUS3', Range) pubUS4[len(pubMarker)]=rospy.Publisher('/relocNode'+str(len(pubMarker))+'/relocUS4', Range) print "New node registered:"+ sourceaddr #publish data of new node Rfdata=response['rf_data'].decode("utf-8") #print(sourceaddr+' :'+Rfdata) #Publish all topics rospy.sleep(0.01)
import rospy from sensor_msgs.msg import Range import Adafruit_BBIO.ADC as ADC import math ADC.setup() if __name__ == '__main__': try: rospy.init_node('irnode') global pub IR = Range() IR.header.frame_id="inertal_link" IR.radiation_type = 1 IR.field_of_view = 3.14/5.0 IR.min_range = 0.20 IR.max_range = 1.5 IR.range = None pub = rospy.Publisher('/range',Range, queue_size=10) while not rospy.is_shutdown(): voltage = 0.0 voltage = ADC.read("P9_39") value = voltage * 5.0 distance = 59.23 * math.pow(value,-1.1597) distance = distance / 100.0 print( distance) IR.range = distance rospy.loginfo(IR) pub.publish(IR) rospy.spin() except rospy.ROSInterruptException: pass
from sensor_msgs.msg import Range from tf.broadcaster import TransformBroadcaster import ranger RATE = 40 #Hz BASE_FRAME = "base_link" range_left = Range() range_left.header.frame_id = "ir_left" range_left.radiation_type = Range.INFRARED range_left.field_of_view = 0.4 # ~20deg range_left.min_range = 0.025 # according to my measurments range_left.max_range = 0.30 # according to my measurments range_center = Range() range_center.header.frame_id = "ir_center" range_center.radiation_type = Range.INFRARED range_center.field_of_view = 0.4 # ~20 deg range_center.min_range = 0.35 # according to my measurments range_center.max_range = 2.0 # according to my measurments range_right = Range() range_right.header.frame_id = "ir_right" range_right.radiation_type = Range.INFRARED range_right.field_of_view = 0.4 # ~20deg range_right.min_range = 0.025 # according to my measurments range_right.max_range = 0.30 # according to my measurments