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 __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 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 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 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 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 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 _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 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 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 get_height(self):
voltage = self.adc.get_last_result()
if voltage <= 0:
voltage = 1
rospy.logerr("ERROR: BAD VOLTAGE!!!")
return
#rospy.loginfo("voltage = " + str(voltage))
self.distance = pow(voltage, self.p) * self.m
#rospy.loginfo("distance = " + str(self.distance))
msg = Range()
msg.max_range = 0.8
msg.min_range = 0
msg.range = self.distance
msg.header.stamp = rospy.Time.now()
self.pub_height.publish(msg)
def sample(self):
data = self.ping.get_distance()
if data:
msg = Range()
msg.radiation_type = 0
msg.min_range = 0.5
msg.max_range = 30.0
msg.range = float(data["distance"]) / 1000.0
msg.field_of_view = float(
data["confidence"]
) / 100.0 # Abusing this field to store the confidence value
msg.header.stamp = self.get_clock().now().to_msg()
self.pub.publish(msg)
else:
print("Failed to get distance data")
def create_ranged_message(distance):
'''
Create a ranged message for publishing sonar values
The field of view takes the dual sonar implementation
in account
'''
message = Range()
message.header.stamp = rospy.Time.now()
message.header.frame_id = 'base_footprint'
message.radiation_type = 0
message.field_of_view = math.pi / 25 # [rad] - 7.2 deg
message.min_range = 0.05 # [m] closest 1 cm
message.max_range = 3.50 # [m] furtheset 100 cm / 1m
message.range = distance - 0.05
return message
def main():
rospy.init_node("infrared_pub")
pub = rospy.Publisher('/pidrone/infrared', Range, queue_size=1)
rnge = Range()
rnge.max_range = 0.8
rnge.min_range = 0
rnge.header.frame_id = "base"
r = rospy.Rate(100)
print "publishing IR"
while not rospy.is_shutdown():
rnge.header.stamp = rospy.get_rostime()
rnge.header.frame_id = "ir_link"
rnge.range = get_range()
pub.publish(rnge)
r.sleep()
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 distance_sonic():
pub = rospy.Publisher('IR_top', Range, queue_size=20)
rospy.init_node('qqqqq', anonymous=True)
rate = rospy.Rate(10)
r = Range()
r.header.frame_id = '/IR_top'
r.field_of_view = 0.26
r.max_range = 4.5
r.min_range = 0.05
print('IR start')
while not rospy.is_shutdown():
r.header.stamp = rospy.Time.now()
r.range = sensor_distance()
pub.publish(r)
rate.sleep()
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 publish_range():
global distance, pub
# compose the message
sensor_range = Range()
sensor_range.header.stamp = rospy.Time.now()
sensor_range.radiation_type = Range.ULTRASOUND
sensor_range.field_of_view = 0.26
sensor_range.min_range = 0
sensor_range.max_range = 2
sensor_range.range = distance/100 # distance in meters
sensor_range.header.frame_id = "dextrobot_front_sonar"
# publish the composed message
pub.publish(sensor_range)
def publish_distance_data(self, time, data):
range_msg = Range()
range_msg.header.stamp = time
range_msg.header.frame_id = "sonar"
range_msg.header.seq = data["ping_number"]
range_msg.radiation_type = Range.ULTRASOUND
range_msg.min_range = data["scan_start"] / 1000.0
range_msg.max_range = (data["scan_start"] +
data["scan_length"]) / 1000.0
range_msg.range = data["distance"] / 1000.0
self.distance_pub.publish(range_msg)
pass
def timer_callback(self): # create the message containing the moving average
for i in range(len(self.sensor)):
# print "publishing"
distance = self.sensor[i].readRangeData()
if (distance < 14000 and distance > 200):
terarangers_msg = Range()
terarangers_msg.header.frame_id = "base_range"
terarangers_msg.header.stamp = rospy.Time.now()
terarangers_msg.radiation_type = 1
terarangers_msg.field_of_view = 0.0593
terarangers_msg.min_range = 200
terarangers_msg.max_range = 14000 # 14 metres
terarangers_msg.range = distance
# publish the moving average
self.range_pub[i].publish(terarangers_msg)
def quad_sub(self, odom):
for tag_name in self.tag_pos.keys():
r = Range()
r.header.frame_id = tag_name
dist = self.distance(self.tag_pos[tag_name],
odom.pose.pose.position)
dist = dist + random.gauss(0, 0.07)
if dist < 0.1:
dist = 0.1
r.range = dist
r.field_of_view = math.pi * 0.1
r.min_range = 0
r.max_range = 300
self.tag_pub[tag_name].publish(r)
self.alt_pub(odom)
self.odom_pub(odom)
def publish():
while not rospy.is_shutdown():
pub = rospy.Publisher("distance", Range, queue_size=10)
rospy.init_node("ultrasonic", anonymous=True)
rate = rospy.Rate(4) #4Hz
msg = Range(radiation_type=0)
## ULTRASOUND =
## IR = 1
msg.range = get_distance()
msg.min_range = min_range
msg.max_range = max_range
if msg.range < max_range and msg.range > min_range:
pub.publish(msg)
rate.sleep()
rospy.loginfo("%.2f" % (msg.range))
def to_msg(self, sensor_id, distance):
msg = Range()
msg.header = Header()
msg.header.stamp = self.get_clock().now().to_msg()
# need to define transforms for all sensors
msg.header.frame_id = f"ultrasonic_sensor_{sensor_id}"
msg.radiation_type = 0
msg.field_of_view = 0.0
msg.min_range = self.min_range
msg.max_range = self.max_range
msg.range = distance
return msg
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 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('range', Range, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(2) # 2hz
msg = Range()
seq = 0
while not rospy.is_shutdown():
seq += 1
msg.header.seq = seq
msg.radiation_type = 0
msg.field_of_view = 30.0
msg.min_range = 0.2
msg.max_range = 2.0
msg.range = random.random() * 2.0
pub.publish(msg)
rate.sleep()
def main():
trig_pin, echo_pin, units = setup()
range_msg = Range()
range_msg.header.frame_id = "ultrasonic_front"
range_msg.radiation_type = 0
range_msg.field_of_view = 0.1 #Fake value
range_msg.min_range = 0.78
range_msg.max_range = 157.48
rate = rospy.Rate(1)
while not rospy.is_shutdown():
distance = get_sonar_readings(trig_pin, echo_pin, units)
# print(distance)
range_msg.header.stamp = rospy.Time.now()
range_msg.range = distance
pub.publish(range_msg)
rate.sleep()
def pointcloud_cb(self, pc: PointCloud2):
"""Process new sensor information of depth camera."""
out_msg = Range()
out_msg.field_of_view = self.param.tof.field_of_view
out_msg.min_range = self.param.tof.min_range
out_msg.max_range = self.param.tof.max_range
out_msg.header.frame_id = self.param.frame_id
vecs = (Vector(p) for p in sensor_msgs.point_cloud2.read_points(
pc, field_names=("x", "y", "z")))
out_msg.range = min(v.norm for v in vecs)
rospy.logdebug(f"Pointcloud received in {rospy.get_name()}:{vecs}")
rospy.logdebug(f"Publishing range: {out_msg.range}")
self.publisher.publish(out_msg)
def talker():
pub = rospy.Publisher('range', Range)
rospy.init_node('talker')
ranges = [float('NaN'), 1.0, -float('Inf'), 3.0, float('Inf')]
min_range = 2.0
max_range = 2.0
while not rospy.is_shutdown():
for rg in ranges:
r = Range()
r.header.stamp = rospy.Time.now()
r.header.frame_id = "/base_link"
r.radiation_type = 0
r.field_of_view = 0.1
r.min_range = min_range
r.max_range = max_range
r.range = rg
pub.publish(r)
rospy.sleep(1.0)
def sensor():
pub_sonar = []
for topic in sonar_topics:
pub_sonar.append(rospy.Publisher(topic, Range, queue_size=10))
rospy.init_node("sensor", anonymous=True)
rate = rospy.Rate(10)
range_msg = Range()
range_msg.field_of_view = 0.05
range_msg.min_range = 0.0
range_msg.max_range = 5.0
while not rospy.is_shutdown():
sonar_data = [200, 200, 200, 200, 1000, 1000, 1000, 1000]
i = 0
print("\033c")
for elem in sonar_data:
range_msg.header.frame_id = sonar_topics[i]
range_msg.range = elem / 1000.0
if elem == 0:
range_msg.range = 5.0
pub_sonar[i].publish(range_msg)
# rospy.loginfo(range_msg.range)
bar = ""
if int(elem) == 0:
for k in range(1, 100):
bar += "*"
for j in range(1, int(elem) / 20):
bar += "-"
if j > 100:
break
rospy.loginfo(bar)
i = i + 1
rate.sleep()
def step(self):
stamp = super().step()
if not stamp:
return
# Publish distance sensor data
if self._publisher.get_subscription_count() > 0 or self._always_publish:
self._wb_device.enable(self._timestep)
msg = Range()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.field_of_view = self._wb_device.getAperture()
msg.min_range = self._min_range
msg.max_range = self._max_range
msg.range = interpolate_lookup_table(self._wb_device.getValue(), self._wb_device.getLookupTable())
msg.radiation_type = Range.INFRARED
self._publisher.publish(msg)
else:
self._wb_device.disable()
def read_pose(self,pos):
'''Callback when new pose topic has come. Updates distance and control signals'''
distance=measure_distance_front((pos.x,pos.y,pos.theta))
range_message=Range()
range_message.header.stamp = rospy.Time.now()
range_message.field_of_view=0.1
range_message.min_range=0
range_message.max_range=10
range_message.range=distance
self.distance_publisher.publish(range_message)
self.latching_controller(distance)
def node_run():
rospy.init_node("UltraSonicSensor")
pub = rospy.Publisher("UltraSonicRange", Range)
#rate for measuring and publishing the distance 10 = 10Hz = 10 times per second
rate = rospy.Rate(10)
data = Range()
while not rospy.is_shutdown():
#measure and publish
data.radiation_type = data.ULTRASOUND
data.field_of_view = 0.0
data.min_range = 0.01
data.max_range = 10.0
#devide by 100, Function returns cm, Message expects metres
data.range = get_distance() / 100.0
pub.publish(data)
rate.sleep()
def __init__(
self,
num_sonar,
gpio_trigger_list, #-- list of all the trigger pins, starting from left
gpio_echo_list, #-- list of all the echo pins, starting from left
range_min, #- [m]
range_max, #- [m]
angle_min_deg, #- [deg]
angle_max_deg):
self.sonar_array = []
self.pub_array = []
self.num_sonar = num_sonar
delta_angle_deg = (angle_max_deg - angle_min_deg) / float(num_sonar -
1)
rospy.loginfo("Initializing the arrays")
#--- Create an array and expand the object with its angle
for i in range(num_sonar):
sonar = Sonar(gpio_trigger_list[i],
gpio_echo_list[i],
range_min=range_min * 100,
range_max=range_max * 100)
angle_deg = angle_min_deg + delta_angle_deg * i
sonar.angle = math.radians(angle_deg)
self.sonar_array.append(sonar)
rospy.loginfo("Sonar %d set" % i)
#--- Publishers
topic_name = "/dkcar/sonar/%d" % i
pub = rospy.Publisher(topic_name, Range, queue_size=5)
self.pub_array.append(pub)
rospy.loginfo("Publisher %d set with topic %s" % (i, topic_name))
#--- Default message
message = Range()
# message.ULTRASOUND = 1
# message.INFRARED = 0
message.radiation_type = 0
message.min_range = range_min
message.max_range = range_max
self._message = message
def start(self):
self.enable = True
self.ultrasonicPub = rospy.Publisher(self.robot_host + '/ultrasonic/front/left/distance', Range, queue_size=10)
self.ultrasonicVelocityPub = rospy.Publisher(self.robot_host + '/ultrasonic/front/left/relative_velocity', RelativeVelocity, queue_size=10)
ultrasonicFrontLeft = UltrasonicHCSR04(23, 24)
#ranges = [float('NaN'), 1.0, -float('Inf'), 3.0, float('Inf')]
min_range = 3
max_range = 400
while not rospy.is_shutdown():
distance = ultrasonicFrontLeft.distance()
relative_velocity = ultrasonicFrontLeft.speed()
message_str = "frontLeftUltrasonic Distance: %s cm and Speed: %s m/s" % (distance, relative_velocity)
rospy.loginfo(message_str)
#for distance in ranges:
r = Range()
rv = RelativeVelocity()
r.header.stamp = rospy.Time.now()
r.header.frame_id = "/base_link"
r.radiation_type = Range.ULTRASOUND
r.field_of_view = 0.26179938779915 # 15 degrees
r.min_range = min_range
r.max_range = max_range
r.range = distance
rv.header.stamp = rospy.Time.now()
rv.header.frame_id = "/base_link"
rv.radiation_type = Range.ULTRASOUND
rv.field_of_view = 0.26179938779915 # 15 degrees
rv.relative_velocity = relative_velocity
self.ultrasonicPub.publish(r)
self.ultrasonicVelocityPub.publish(rv)
self.rate.sleep()
def start(self):
self.enable = True
self.infraredPub = rospy.Publisher(self.robot_host + '/infrared/rear/distance', Range, queue_size=10)
self.infraredVelocityPub = rospy.Publisher(self.robot_host + '/infrared/rear/relative_velocity', RelativeVelocity, queue_size=10)
infrared = IRGP2Y0A02YKOF(1)
#ranges = [float('NaN'), 1.0, -float('Inf'), 3.0, float('Inf')]
min_range = 20
max_range = 150
while not rospy.is_shutdown():
distance = infrared.distance()
relative_velocity = infrared.speed()
message_str = "rearInfrared Distance: %s cm and Speed: %s m/s" % (distance, relative_velocity)
rospy.loginfo(message_str)
#for distance in ranges:
r = Range()
rv = RelativeVelocity()
r.header.stamp = rospy.Time.now()
r.header.frame_id = "/base_link"
r.radiation_type = Range.INFRARED
r.field_of_view = 0.087266462599716 # 5 degrees
r.min_range = min_range
r.max_range = max_range
r.range = distance
rv.header.stamp = rospy.Time.now()
rv.header.frame_id = "/base_link"
rv.radiation_type = Range.INFRARED
rv.field_of_view = 0.087266462599716 # 5 degrees
rv.relative_velocity = relative_velocity
self.infraredPub.publish(r)
self.infraredVelocityPub.publish(rv)
self.rate.sleep()
def ultrasonic_range():
global ranger
pub = rospy.Publisher('range', Range, queue_size=1)
# 5Hz / Sensor could support 10Hz but the process
# would use a lot of CPU in bad cases
rate = rospy.Rate(10)
range = Range()
range.header.frame_id = "range_forward"
range.radiation_type = 0
range.field_of_view = 20
range.min_range = 0.02
range.max_range = 4.00
while not rospy.is_shutdown():
range.range = ranger.get_distance_cm() / 100
pub.publish(range)
rate.sleep()
rospy.on_shutdown(shutdown_ranger)
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 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 run_lidar(self, event):
if self.ser.in_waiting >= 9:
if (b'Y' == self.ser.read()) and ( b'Y' == self.ser.read()):
dist_l = self.ser.read()
dist_h = self.ser.read()
self.distance = (ord(dist_h) * 256) + (ord(dist_l))
strength_l = self.ser.read()
strength_h = self.ser.read()
self.strength = (ord(strength_h) * 256) + (ord(strength_l))
for i in range (0, 3):
self.ser.read()
distance_msg = Range()
distance_msg.range = float(self.distance) / 100
distance_msg.min_range = 0.30
distance_msg.max_range = 7.0
self.distance_pub.publish(distance_msg)
print (self.distance, self.strength)
self.ser.flush()
def add_range_measurement(self, new_range_message, sensor):
minimum_fov = 0.001
self.last_measurements[sensor] = [new_range_message.range
] + self.last_measurements[sensor]
del self.last_measurements[sensor][10:]
# Calculating the mean and variance
self.avg_measurement[sensor] = sum(
self.last_measurements[sensor]) / len(
self.last_measurements[sensor])
sum_of_squared_error = 0
for measurement in self.last_measurements[sensor]:
sum_of_squared_error += (measurement -
self.avg_measurement[sensor])**2
self.variance[sensor] = sum_of_squared_error / len(
self.last_measurements[sensor])
# Creates a range message
range_and_variance_message = Range()
range_and_variance_message.radiation_type = Range.ULTRASOUND
range_and_variance_message.header.frame_id = "/ultrasound/" + sensor
range_and_variance_message.min_range = 25.0
range_and_variance_message.max_range = 500.0
range_and_variance_message.range = self.avg_measurement[sensor]
range_and_variance_message.field_of_view = np.clip(
self.variance[sensor] / 1000, 0,
10) # Using FOV as variance with a max limit at ten
# Publishing on the right topic
if sensor == "left":
self.left_measure_and_variance_pub.publish(
range_and_variance_message)
elif sensor == "center":
self.center_measure_and_variance_pub.publish(
range_and_variance_message)
elif sensor == "right":
self.right_measure_and_variance_pub.publish(
range_and_variance_message)
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 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 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 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
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
def talker():
#tcp config
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((TCP_IP, TCP_PORT))
pubMarker = {} # publisher dictionary
pubText = {} # publisher dictionary
pubUS1 = {}
pubUS2 = {}
pubUS3 = {}
pubUS4 = {}
rospy.init_node('RelocNode')
#debug topics
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)
#The published topics
pubText=rospy.Publisher('relocNode/relocVizDataText', Marker)
pubMarkerRange=rospy.Publisher('relocNode/relocVizDataRange', Marker)
pubMarkerBear=rospy.Publisher('relocNode/relocVizDataBear', Marker)
pubMarkerBear2=rospy.Publisher('relocNode/relocVizDataBear2', Marker)
pubMarkerBear3=rospy.Publisher('relocNode/relocVizDataBear3', Marker)
pubUSAoa = rospy.Publisher('relocNode/relocUsAoa', Range)
pubUSAoa2 = rospy.Publisher('relocNode/relocUsAoa2', Range)
#Us range sensors
pubUS1=rospy.Publisher('relocNode/relocUS1', Range)
pubUS2=rospy.Publisher('relocNode/relocUS2', Range)
pubUS3=rospy.Publisher('relocNode/relocUS3', Range)
pubUS4=rospy.Publisher('relocNode/relocUS4', Range)
#parameter for tf prefix
if rospy.has_param("~tf_prefix"):
tf_prefix = rospy.get_param("~tf_prefix")
else:
tf_prefix= ""
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
# initialy flush the buffer
response = s.recv(BUFFER_SIZE)
while not rospy.is_shutdown():
response = s.recv(BUFFER_SIZE)
# check if its a new source address and initialize publishers
Rfdata=response
#print response
str2=Rfdata.split(',')
#packet data extraction
range_flag =0
bearing_flag =0
packetValid =0
new_packet =0
robId="??"
try:
for i in range(len(str2)):
if str2[i][0:2] == "RL" or str2[i][0:2] == "rl":
robID = str2[i][2:]
localID = str2[i][2]
targetID = str2[i][4]
#print localID,targetID
#transmit the previous set
if str2[i] == "IR" or 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
packetValid=1
if str2[i] == "ANG" or str2[i] == "ang":
#print str2[i+1]
IRang=int(str2[i+1])-90
#print IRang/180.0*math.pi
br.sendTransform((0.085, 0, -0.035),tf.transformations.quaternion_from_euler(0, 0, -IRang/180.0*math.pi),rospy.Time.now(),tf_prefix+"/IRcam",tf_prefix+"/base_reloc2")
br.sendTransform((0.085, 0, -0.035),tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),tf_prefix+"/IRcamBase",tf_prefix+"/base_reloc2")
#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" or 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"
print str2,robID
packetValid=0
#Measurement processing
if packetValid==1:
#print targetID,US1,US2,US3,US4,IRx1,IRang
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 slog
#print USmin
#print IRx1
if IRx1==1023 or bearing_flag==0:
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
#Data publish
text_marker = Marker()
text_marker.header.frame_id=tf_prefix+"/base_reloc2"
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.text = "%s%s" % (localID,targetID)
text_marker.pose.position.x= 0
text_marker.pose.position.y= 0
text_marker.pose.position.z= 0
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" or "Range Bearing":
arrow_marker = Marker()
arrow_marker.header.frame_id=tf_prefix+"/base_reloc2"
arrow_marker.ns="%s%s" % (localID,targetID)
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
if slog == "Bearing only" or "Range Bearing":
arrow_marker2 = Marker()
#arrow_marker2.header.frame_id=tf_prefix+"/base_reloc2"
arrow_marker2.header.frame_id=tf_prefix+"/IRcam"
arrow_marker2.ns="%s%s" % (localID,targetID)
arrow_marker2.type = Marker.ARROW
arrow_marker2.points = {Point(0,0,0),Point(robx,roby,robz)}
arrow_marker2.scale.x = 0.1
arrow_marker2.scale.y = 0.1
arrow_marker2.scale.z = 0.2
arrow_marker2.color.r = 1.0
arrow_marker2.color.g = 0.5
arrow_marker2.color.b = 0.5
arrow_marker2.color.a = 0.5
arrow_marker3 = Marker()
#arrow_marker2.header.frame_id=tf_prefix+"/base_reloc2"
arrow_marker3.header.frame_id=tf_prefix+"/IRcamBase"
arrow_marker3.ns="%s%s" % (localID,targetID)
arrow_marker3.type = Marker.ARROW
arrow_marker3.points = {Point(0,0,0),Point(robx,roby,robz)}
arrow_marker3.scale.x = 0.1
arrow_marker3.scale.y = 0.1
arrow_marker3.scale.z = 0.2
arrow_marker3.color.r = 1.0
arrow_marker3.color.g = 0.5
arrow_marker3.color.b = 0.5
arrow_marker3.color.a = 0.5
arrow_marker4 = Marker()
arrow_marker4.header.frame_id=tf_prefix+"/IRcamBase"
arrow_marker4.ns="%s%s" % (localID,targetID)
arrow_marker4.type = Marker.ARROW
arrow_marker4.points = {Point(0,0,0),Point(robx,-roby,robz)}
arrow_marker4.scale.x = 0.1
arrow_marker4.scale.y = 0.1
arrow_marker4.scale.z = 0.2
arrow_marker4.color.r = 1.0
arrow_marker4.color.g = 0.5
arrow_marker4.color.b = 0.5
arrow_marker4.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
if slog == "Range only" or "Range Bearing":
USang1=0.0
USarr=[US1/1000.0,US2/1000.0,US3/1000.0,US4/1000.0]
#print USarr
USIds=[0,1,2,3]
USmin1=min(USarr)
USminId=USarr.index(min(USarr))
USIds.remove(USminId)
del USarr[USminId]
if USminId==0: USang1=90.0
if USminId==1: USang1=-90.0
if USminId==2: USang1=0.0
if USminId==3: USang1=180.0
USminId=USarr.index(min(USarr))
USmin2=min(USarr)
if USminId==0: USang2=90.0
if USminId==1: USang2=-90.0
if USminId==2: USang2=0.0
if USminId==3: USang2=180.0
corr=(USmin2-USmin1)*45/0.1
if corr>45:corr=45
if corr<-45:corr=-45
#print USmin2-USmin1
USang =(USang1+USang2)/2-corr
#print (USang1+USang2)/2-corr
#print USang1
#print USang2
#print "------------"
#angular coorection
br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, USang1*math.pi/180.0),rospy.Time.now(),tf_prefix+"/base_reloc2/USAoa",tf_prefix+"/base_reloc2")
USAoa = Range()
USAoa.header.frame_id=tf_prefix+"/base_reloc2/USAoa"
USAoa.radiation_type=0
USAoa.field_of_view =math.pi/4.0
USAoa.min_range = 0.5
USAoa.max_range = 10.0
USAoa.range = USmin
br.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(0, 0, -USang1*math.pi/180.0),rospy.Time.now(),tf_prefix+"/base_reloc2/USAoa2",tf_prefix+"/base_reloc2")
USAoa2 = Range()
USAoa2.header.frame_id=tf_prefix+"/base_reloc2/USAoa2"
USAoa2.radiation_type=0
USAoa2.field_of_view =math.pi/4.0
USAoa2.min_range = 0.5
USAoa2.max_range = 10.0
USAoa2.range = USmin
USdebug = Quaternion()
USdebug.x = US1
USdebug.y = US2
USdebug.z = US3
USdebug.w = US4
USrange1 = Range()
USrange1.header.frame_id=tf_prefix+"/base_reloc2/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(),tf_prefix+"/base_reloc2/US1",tf_prefix+"/base_reloc2")
USrange2 = Range()
USrange2.header.frame_id=tf_prefix+"/base_reloc2/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(),tf_prefix+"/base_reloc2/US2",tf_prefix+"/base_reloc2/US1")
USrange3 = Range()
USrange3.header.frame_id=tf_prefix+"/base_reloc2/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(),tf_prefix+"/base_reloc2/US3",tf_prefix+"/base_reloc2/US1")
USrange4 = Range()
USrange4.header.frame_id=tf_prefix+"/base_reloc2/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(),tf_prefix+"/base_reloc2/US4",tf_prefix+"/base_reloc2/US1")
#print USdebug
#print arrow_marker
pubIRdata1.publish(IRdebug1)
pubIRdata2.publish(IRdebug2)
pubIRdata3.publish(IRdebug3)
pubIRdata4.publish(IRdebug4)
pubUSdata.publish(USdebug)
pubText.publish(text_marker)
pubMarkerRange.publish(arrow_marker)
pubMarkerBear.publish(arrow_marker2)
pubMarkerBear2.publish(arrow_marker3)
pubMarkerBear3.publish(arrow_marker4)
pubUS1.publish(USrange1)
pubUS2.publish(USrange2)
pubUS3.publish(USrange3)
pubUS4.publish(USrange4)
pubUSAoa.publish(USAoa)
pubUSAoa2.publish(USAoa2)
#!/usr/bin/python
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:
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)
try:
vals = [float(tok) for tok in l.strip().split()]
except Exception as e:
rospy.logerr('Failed to parse line from arduino board: %s' % l.strip())
rospy.logerr(e)
continue
if len(vals) != 4:
rospy.logerr('Line from arduino board has wrong number of tokens: %s' % l.strip())
continue
# Sharp 2D120X (4-30 cm range)
ir1_range = Range()
ir1_range.radiation_type = Range.INFRARED
ir1_range.min_range = 0.04
ir1_range.max_range = 0.30
ir1_range.range = ir1_value_to_range(vals[0])
# Sharp 2Y0A21 (10-80 cm range)
ir2_range = Range()
ir2_range.radiation_type = Range.INFRARED
ir2_range.min_range = 0.10
ir2_range.max_range = 0.80
ir2_range.range = ir2_value_to_range(vals[1])
# HRLV-MaxSonar-EZ MB1043 reports anything below 30 cm as 30 cm
# inaccurate readings closer than 50 cm
sonar1_range = Range()
sonar1_range.radiation_type = Range.ULTRASOUND
sonar1_range.min_range = 0
