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