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 sensor_callback(self): self.rightDistance_sensor.enable(self.timestep) msg_rightD = Range() msg_rightD.range = self.rightDistance_sensor.getValue() self.sensorPublisher_rightDistance.publish(msg_rightD) msg_leftD = Range() self.leftDistance_sensor.enable(self.timestep) msg_leftD.range = self.leftDistance_sensor.getValue() self.sensorPublisher_leftDistance.publish(msg_leftD) msg_frontD = Range() self.frontDistance_sensor.enable(self.timestep) msg_frontD.range = self.frontDistance_sensor.getValue() self.sensorPublisher_frontDistance.publish(msg_frontD) msg_rightP = Float64() self.rightPosition_sensor.enable(self.timestep) msg_rightP.data = self.rightPosition_sensor.getValue() self.sensorPublisher_rightPosition.publish(msg_rightP) msg_leftP = Float64() self.leftPosition_sensor.enable(self.timestep) msg_leftP.data = self.leftPosition_sensor.getValue() self.sensorPublisher_leftPosition.publish(msg_leftP)
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 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 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)
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 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 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 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 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(): 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 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 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 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 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 rangeCB(self, data): range_msg = Range() range_msg.header.stamp = rospy.Time.now() range_msg.radiation_type = Range.ULTRASOUND range_msg.range = float(data[1]) range_msg.field_of_view = float(data[0]) self.range_pub.publish(range_msg)
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 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 publish_dist_to_target(self, target): ''' Publish distance to target (timer callback) ''' r = Range() r.header.stamp = rospy.Time.now() r.range = self.custom_uwb_range self.dist_to_target_pub.publish(r)
def publish_ir(self): ''' Publish simulated IR measurements ''' range_msg = Range() range_msg.header.stamp = rospy.Time.now() range_msg.range = self.z_pos_ir_measurement self.ir_pub.publish(range_msg)
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 right_distance_cb(self, MsgIn): '''creates range with params predefined on top and publishes''' self.right_distance_val = MsgIn.data range = Range(**sensor_params) range.header.frame_id = sensor_names['right'] range.header.stamp = rospy.Time.now() range.range = MsgIn.data self.right_distance_pub.publish(range)
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 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 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 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 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 parse_sensor_data(line): global scan_pub, range_pub # Handle debug text from the arduino if "," not in line: rospy.loginfo("RangeNode: %s" % line) return # Parse the range string into a float array ranges = [float(x) / 1000.0 for x in line.split(",")[::-1]] if len(ranges) != 8: rospy.logwarn("Received other than 8 scan ranges", ranges) return br = tf2_ros.TransformBroadcaster() # msg = LaserScan() # msg.header.frame_id = "base_link" # msg.header.stamp = rospy.Time.now() # msg.angle_increment = 26.0/180.0*3.141592 # msg.angle_min = msg.angle_increment*-3.5 # msg.angle_max = msg.angle_increment*3.5 # msg.range_min = 0.1 # msg.range_max = 4.0 # msg.ranges = ranges # reverse! # scan_pub.publish(msg) for i in range(0, 8): # Emit the range data for this range rmsg = Range() rmsg.header.frame_id = "base_link" rmsg.header.stamp = rospy.Time.now() rmsg.header.frame_id = "rangefinder_%d" % i rmsg.min_range = 0.1 rmsg.max_range = 4.0 rmsg.field_of_view = 26.0 / 180.0 * 3.141592 rmsg.radiation_type = rmsg.INFRARED rmsg.range = ranges[i] range_pub[i].publish(rmsg) # output the TF2 for this range t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "base_footprint" t.child_frame_id = rmsg.header.frame_id t.transform.translation.x = range_pos[i][0] t.transform.translation.y = range_pos[i][1] - 0.2 t.transform.translation.z = 0.2 q = tf_conversions.transformations.quaternion_from_euler( 0, -range_pos[i][3] / 180.0 * 3.1415, range_pos[i][2] / 180.0 * 3.1415 - 3.1415 / 2) t.transform.rotation.x = q[0] t.transform.rotation.y = q[1] t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] br.sendTransform(t)
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 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 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 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 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 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 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 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 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 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 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 talker(): pub = rospy.Publisher('lidar_data', Range, queue_size=10) rospy.init_node('lidar_node') rate = rospy.Rate(1000) # 10hz ser=serial.Serial('/dev/ttyUSB1',115200) msg=Range() while not rospy.is_shutdown(): p=ser.readline() if p!='\n' and p!='': try: msg.range=int(p) msg.header.stamp = rospy.Time.now() print msg.range pub.publish(msg) except: pass rate.sleep()
def node(): rospy.init_node('sonar_node', anonymous=True) range_msg = Range() range_msg.radiation_type = range_msg.ULTRASOUND pub = rospy.Publisher("/msg_from_sonar", Range, queue_size=10) r = rospy.Rate(20) rospy.loginfo("reading sonar range") while not rospy.is_shutdown(): #trigger sonar to measure cmd = chr(0x22)+chr(0x00)+chr(0x00)+chr(0x22) ser.write(cmd) data = ser.read(4) if len(data)==4 and (ord(data[0])+ord(data[1])+ord(data[2]))&255 == ord(data[3]): sonar_range = ord(data[1])*256+ord(data[2]) range_msg.range = sonar_range pub.publish(range_msg) rospy.loginfo("distance : %d", sonar_range) else: rospy.logwarn("sonar error!") r.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 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 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()
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