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 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 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
