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 lazer_msg_constructor(self, topic_name, lazer_frame, distance, angle):
r = Range()
r.header.stamp = rospy.Time.now()
r.header.frame_id = lazer_frame
r.radiation_type = 0
r.field_of_view = 0.8
r.min_range = 0
r.max_range = 10000
r.radiation_type = 1
r.range = distance
scan_pub = rospy.Publisher(topic_name, Range, queue_size=10)
scan_pub.publish(r)
time.sleep(0.05)
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 __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 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 __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 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_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 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 rangeCB(self, data):
range_msg = Range()
range_msg.header.stamp = rospy.Time.now()
range_msg.radiation_type = Range.ULTRASOUND
range_msg.range = float(data[1])
range_msg.field_of_view = float(data[0])
self.range_pub.publish(range_msg)
def distance_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 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 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 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 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 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 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 parse_sensor_data(line):
global scan_pub, range_pub
# Handle debug text from the arduino
if "," not in line:
rospy.loginfo("RangeNode: %s" % line)
return
# Parse the range string into a float array
ranges = [float(x) / 1000.0 for x in line.split(",")[::-1]]
if len(ranges) != 8:
rospy.logwarn("Received other than 8 scan ranges", ranges)
return
br = tf2_ros.TransformBroadcaster()
# msg = LaserScan()
# msg.header.frame_id = "base_link"
# msg.header.stamp = rospy.Time.now()
# msg.angle_increment = 26.0/180.0*3.141592
# msg.angle_min = msg.angle_increment*-3.5
# msg.angle_max = msg.angle_increment*3.5
# msg.range_min = 0.1
# msg.range_max = 4.0
# msg.ranges = ranges # reverse!
# scan_pub.publish(msg)
for i in range(0, 8):
# Emit the range data for this range
rmsg = Range()
rmsg.header.frame_id = "base_link"
rmsg.header.stamp = rospy.Time.now()
rmsg.header.frame_id = "rangefinder_%d" % i
rmsg.min_range = 0.1
rmsg.max_range = 4.0
rmsg.field_of_view = 26.0 / 180.0 * 3.141592
rmsg.radiation_type = rmsg.INFRARED
rmsg.range = ranges[i]
range_pub[i].publish(rmsg)
# output the TF2 for this range
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "base_footprint"
t.child_frame_id = rmsg.header.frame_id
t.transform.translation.x = range_pos[i][0]
t.transform.translation.y = range_pos[i][1] - 0.2
t.transform.translation.z = 0.2
q = tf_conversions.transformations.quaternion_from_euler(
0, -range_pos[i][3] / 180.0 * 3.1415,
range_pos[i][2] / 180.0 * 3.1415 - 3.1415 / 2)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
br.sendTransform(t)
def define_cone(frame_id):
cone = Range()
cone.header.frame_id = frame_id
cone.field_of_view = pi * friction_angle*2 / 180
cone.radiation_type = 1
cone.min_range = 0.05
cone.max_range = 0.1
cone.range = 0.05
return cone
def pub_laser(self, dist):
laser_msg = Range()
laser_msg.header.stamp = rospy.Time.now()
laser_msg.range = dist
laser_msg.min_range = 0.5
laser_msg.max_range = 12.
laser_msg.field_of_view = 0.0349
laser_msg.radiation_type = 1
self._laser_pub.publish(laser_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 start(self):
self.enable = True
self.ultrasonicPub = rospy.Publisher(self.robot_host + '/ultrasonic/front/distance', Range, queue_size=10)
self.ultrasonicVelocityPub = rospy.Publisher(self.robot_host + '/ultrasonic/rear/relative_velocity', RelativeVelocity, queue_size=10)
ultrasonic = UltrasonicParallax(27)
#ranges = [float('NaN'), 1.0, -float('Inf'), 3.0, float('Inf')]
min_range = 3
max_range = 300
while not rospy.is_shutdown():
distance = ultrasonic.distance()
relative_velocity = ultrasonic.speed()
# status = ultrasonic.less_than(threshold)
# if distance != -1:
# print('distance', distance, 'cm')
# time.sleep(0.2)
# else:
# print(False)
# if status == 1:
# print("Less than %d" % threshold)
# elif status == 0:
# print("Over %d" % threshold)
# else:
# print("Read distance error.")
message_str = "frontUltrasonic 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.34906585039887 # 20 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.34906585039887 # 20 degrees
rv.relative_velocity = relative_velocity
self.ultrasonicPub.publish(r)
self.ultrasonicVelocityPub.publish(rv)
self.rate.sleep()
def talker():
signal.signal(signal.SIGINT, signal_handler)
time.sleep(1)
setup_servo()
pub = rospy.Publisher('servo_ulta', Range, queue_size=10)
rospy.init_node('servo_ultra', anonymous=True, disable_signals=False)
rate = rospy.Rate(10) # 10hz
ultrasonic_number = 1
pause_time = 0.1
delay_ultrasonic = 0.11
STEP_ANGLE = 10
CURR_ANGLE = (90.0 * math.pi) / 180.0
mindt = 0.65
maxdt = 2.48
dt = [mindt, maxdt]
extremes = [int(x * 1024.0 / 20.0) for x in dt]
dt_range = extremes[1] - extremes[0]
dt_step = int((dt_range / 180.0) * STEP_ANGLE)
br = tf.TransformBroadcaster()
while not rospy.is_shutdown():
#broadcast_transforms()
# Move servo to counter-clockwise extreme.
wiringpi.pwmWrite(18, extremes[1])
sleep(0.2)
CURR_ANGLE = (90.0 * math.pi) / 180.0
for i in range(extremes[1], extremes[0],
-dt_step): # 1025 because it stops at 1024
wiringpi.pwmWrite(18, i)
sleep(pause_time)
br.sendTransform((0.15, 0.0, 0.0),
tf.transformations.quaternion_about_axis(
CURR_ANGLE, (0, 0, 1)), rospy.Time.now(), "1",
"base_link")
data = Range()
data.header.frame_id = "1"
data.header.stamp = rospy.Time.now()
data.radiation_type = 0
data.min_range = 0.05
data.max_range = 2.0
data.field_of_view = 0.164
try:
data.range = float(get_us_data_from(ultrasonic_number)) / 100.0
except IOError:
subprocess.call(['i2cdetect', '-y', '1'])
data.range = 0.0
pub.publish(data)
CURR_ANGLE -= (STEP_ANGLE * math.pi) / 180.0
def publish_range(index):
range = Range()
range.header.frame_id = 'world'
range.header.stamp = rospy.get_rostime()
range.radiation_type = 0
range.field_of_view = (math.pi / 3)
range.min_range = 0.1
range.max_range = 2
range.range = 1.5
sensor_publisher[index].publish(range)
def CreateRangeMessage(rng, img_time, sequence):
msg_range = Range()
msg_range.header.frame_id = "world"
msg_range.header.seq = sequence
msg_range.header.stamp = img_time
msg_range.radiation_type = 1
msg_range.field_of_view = 0
msg_range.min_range = 0
msg_range.max_range = 1000000
msg_range.range = rng
return msg_range
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 __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(2)
topic_name_f = '/sonarTP_F'
self.distance_publisher_front = rospy.Publisher(topic_name_f,
Range,
queue_size=5)
#self.r = rospy.Rate(2)
topic_name_r = '/sonarTP_R'
self.distance_publisher_right = rospy.Publisher(topic_name_r,
Range,
queue_size=5)
#self.r = rospy.Rate(2)
topic_name_b = '/sonarTP_B'
self.distance_publisher_back = rospy.Publisher(topic_name_b,
Range,
queue_size=5)
#self.r = rospy.Rate(2)
topic_name_l = '/sonarTP_L'
self.distance_publisher_left = rospy.Publisher(topic_name_l,
Range,
queue_size=5)
self.r = rospy.Rate(2)
self.serialPort0 = "/dev/ttyUSB0"
self.serialPort1 = "/dev/ttyUSB1"
self.serialPort2 = "/dev/ttyUSB2"
self.serialPort3 = "/dev/ttyUSB3"
self.serialPort4 = "/dev/ttyUSB4"
self.maxRange = 2000 # change for 5m vs 10m sensor
self.sleepTime = 0.5
self.minMM = 0.3
self.maxMM = 3
self.mm = float
self.frames = [
'/sonarD_link', '/sonarF_link', '/sonarL_link', '/sonarB_link',
'/sonarR_link'
]
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_ground_sensor_data(self, stamp):
for idx in self.ground_sensors.keys():
msg = Range()
msg.header.stamp = stamp
msg.header.frame_id = idx
msg.field_of_view = self.ground_sensors[idx].getAperture()
msg.min_range = GROUND_MIN_RANGE
msg.max_range = GROUND_MAX_RANGE
msg.range = interpolate_table(
self.ground_sensors[idx].getValue(), GROUND_TABLE)
msg.radiation_type = Range.INFRARED
self.ground_sensor_publishers[idx].publish(msg)
def composeRangeMessage(distance):
sensor_range = Range()
# compose the message
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
return sensor_range
def default(self, ci="unused"):
msg = Range()
msg.radiation_type = Range.INFRARED
msg.field_of_view = 20
msg.min_range = 0
msg.max_range = self.component_instance.bge_object["laser_range"]
tmp = msg.max_range
for ray in self.data["range_list"]:
if tmp > ray:
tmp = ray
msg.range = tmp
self.publish(msg)
def publish_ultrasonic_messages(data_publishers, measured_distances):
range_msg = Range()
range_msg.radiation_type = 0 # ULTRASOUND
range_msg.field_of_view = math.pi / 6.0 # 30 degrees
range_msg.min_range = 0.01
range_msg.max_range = 2.5
for index, measured_distance in enumerate(measured_distances):
range_msg.range = measured_distance
range_msg.header.frame_id = range_sensors_frame_ids[index]
range_msg.header.stamp = rospy.Time.now()
data_publishers[index].publish(range_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'):
""" Publish the data of the infrared sensor as a ROS Range message """
msg = Range()
msg.radiation_type = Range.INFRARED
msg.field_of_view = 20
msg.min_range = 0
msg.max_range = self.component_instance.bge_object['laser_range']
tmp = msg.max_range
for ray in self.data['range_list']:
if tmp > ray:
tmp = ray
msg.range = tmp
self.publish(msg)
def default(self, ci='unused'):
msg = Range()
msg.header = self.get_ros_header()
msg.radiation_type = Range.INFRARED
msg.field_of_view = 20
msg.min_range = 0
msg.max_range = self.component_instance.bge_object['laser_range']
tmp = msg.max_range
for ray in self.data['range_list']:
if tmp > ray:
tmp = ray
msg.range = tmp
self.publish(msg)
def inputCallback(self, msg):
#########################################################################
# rospy.loginfo("-D- range_filter inputCallback")
cur_val = msg.value
if cur_val <= self.max_valid and cur_val >= self.min_valid:
self.prev.append(cur_val)
del self.prev[0]
p = array(self.prev)
self.rolling_ave = p.mean()
self.rolling_std = p.std()
self.rolling_meters = ((self.b * self.rolling_ave) ** self.m) / 100
self.filtered_pub.publish( self.rolling_meters )
self.std_pub.publish( self.rolling_std )
rng = Range()
rng.radiation_type = 1
rng.min_range = self.min_range
rng.max_range = self.max_range
rng.range = self.rolling_meters
rng.header.frame_id = self.frame
rng.field_of_view = 0.1
rng.header.stamp = rospy.Time.now()
self.range_pub.publish( rng )
ranges = []
intensities = []
angle_start = 0.0 - self.field_of_view
angle_stop = self.field_of_view
for angle in linspace( angle_start, angle_stop, 10):
ranges.append( self.rolling_meters )
intensities.append( 1.0 )
scan = LaserScan()
scan.ranges = ranges
scan.header.frame_id = self.frame
scan.time_increment = 0;
scan.range_min = self.min_range
scan.range_max = self.max_range
scan.angle_min = angle_start
scan.angle_max = angle_stop
scan.angle_increment = (angle_stop - angle_start) / 10
scan.intensities = intensities
scan.scan_time = self.scan_time
scan.header.stamp = rospy.Time.now()
self.scan_pub.publish( scan )
def publish(self, data):
msg = Range()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = self._frameId
if self._urfType == URF_HRLV:
msg.min_range = MIN_RANGE_URF_HRLV_MaxSonar
msg.max_range = MAX_RANGE_URF_HRLV_MaxSonar
msg.field_of_view = FIELD_OF_VIEW_URF_HRLV_MaxSonar
else:
msg.min_range = MIN_RANGE_URF_LV_MaxSonar
msg.max_range = MAX_RANGE_URF_LV_MaxSonar
msg.field_of_view = FIELD_OF_VIEW_URF_LV_MaxSonar
msg.radiation_type = Range.ULTRASOUND
msg.range = data
self._pub.publish(msg)
def talker():
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 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 node():
rospy.init_node('sonar_node', anonymous=True)
range_msg = Range()
range_msg.radiation_type = range_msg.ULTRASOUND
pub = rospy.Publisher("/msg_from_sonar", Range, queue_size=10)
r = rospy.Rate(20)
rospy.loginfo("reading sonar range")
while not rospy.is_shutdown():
#trigger sonar to measure
cmd = chr(0x22)+chr(0x00)+chr(0x00)+chr(0x22)
ser.write(cmd)
data = ser.read(4)
if len(data)==4 and (ord(data[0])+ord(data[1])+ord(data[2]))&255 == ord(data[3]):
sonar_range = ord(data[1])*256+ord(data[2])
range_msg.range = sonar_range
pub.publish(range_msg)
rospy.loginfo("distance : %d", sonar_range)
else:
rospy.logwarn("sonar error!")
r.sleep()
def 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 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():
#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/env python
#vim /etc/puppet/manifests/site.pp
import tf
import rospy
from sensor_msgs.msg import Range
if __name__ == '__main__':
rospy.init_node('rangePub')
pub = rospy.Publisher('/vision_range', Range)
r = rospy.Rate(50)
print "gonna start publishin'"
while not rospy.is_shutdown():
vision_range = Range()
vision_range.header.frame_id = "/high_def_frame"
# vision_range.header.stamp = rospy.Time.now()
vision_range.radiation_type = 0
vision_range.field_of_view = 0.5
vision_range.min_range = 0.0
vision_range.max_range = 100.0
vision_range.range = 1.0
pub.publish(vision_range)
# tf_sub = rospy.Subscriber("/tf", tf.msg.tfMessage, tf_callback)
rospy.spin()
print l
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
import rospy 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
#!/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:
if sonar_range < min_ranges[sonar_type]:
print "lower than min range of", min_ranges[sonar_type]
continue
if sonar_range > max_ranges[sonar_type]:
print "higher than max range of", max_ranges[sonar_type]
continue
marker_trans = scipy.matrix([[0., 0., 1., sonar_range],
[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 0., 1.]])
marker_mat = sonar_mats[sonar_id] * marker_trans
range_msg = Range()
range_msg.radiation_type = range_msg.ULTRASOUND
range_msg.field_of_view = angle_spreads[sonar_type]
range_msg.min_range = min_ranges[sonar_type]
range_msg.max_range = max_ranges[sonar_type]
range_msg.range = sonar_range
range_msg.header.stamp = rospy.Time.now()
range_msg.header.frame_id = "sonar" + str(sonar_id)
sonar_pub.publish(range_msg)
sonar_pos, sonar_quat = mat_to_pos_and_quat(sonar_mats[sonar_id])
tf_broadcaster.sendTransform(sonar_frame_pos, sonar_frame_quat, rospy.Time.now(), "sonar_frame", "base_footprint" )
tf_broadcaster.sendTransform(sonar_pos, sonar_quat, rospy.Time.now(), "sonar" + str(sonar_id), "sonar_frame" )
# draw_funcs.draw_rviz_cylinder(marker_mat, sonar_range * math.tan(angle_spreads[sonar_type]/2.),
def sendRange(pub, radiation_type, r):
msg = Range()
msg.radiation_type = radiation_type
msg.range = r
pub.publish(msg)
def run(self):
L = Range()
R = Range()
L.min_range = 0.1
L.max_range = 1.0
L.radiation_type = Range.INFRARED
L.field_of_view = 0.4 # ~22degrees
L.header.frame_id = 'L'
R.min_range = L.min_range
R.max_range = L.max_range
R.radiation_type = L.radiation_type
R.field_of_view = L.field_of_view
R.header.frame_id = 'R'
C = Range()
C.min_range = 0.1
C.max_range = 3.0
C.radiation_type = Range.ULTRASOUND
C.field_of_view = 0.4 # ~22degrees
C.header.frame_id = 'C'
O = Odometry()
O.header.frame_id = 'odom'
O.child_frame_id = 'base_link'
NaN = float('nan')
r = rospy.Rate(self.rate)
lastSensorCommandSent = None
lastTfCommandSent = None
while not rospy.is_shutdown():
if lastSensorCommandSent is None or rospy.get_rostime().to_sec()-lastSensorCommandSent > 5:
self.serial.addCommand('R L R C O\n')
lastSensorCommandSent = rospy.get_rostime().to_sec()
if lastTfCommandSent is None or rospy.get_rostime().to_sec()-lastTfCommandSent > 1:
self.pub_tf.sendTransform((0.09, 0.00, 0.10), tf.transformations.quaternion_about_axis(0.0, (0, 0, 1)), rospy.get_rostime(), 'C', 'base_link')
self.pub_tf.sendTransform((0.07, 0.05, 0.05), tf.transformations.quaternion_about_axis(0.6, (0, 0, 1)), rospy.get_rostime(), 'L', 'base_link')
self.pub_tf.sendTransform((0.07, -0.05, 0.05), tf.transformations.quaternion_about_axis(-0.6, (0, 0, 1)), rospy.get_rostime(), 'R', 'base_link')
lastTfCommandSent = rospy.get_rostime().to_sec()
'''
#code to dynamically enable/disable sensors depending on number of subscribers, disabled for now
if lastSensorCommandSent==None or rospy.get_rostime().to_sec()-lastSensorCommandSent>5:
cmdOn = 'R '
cmdOff = 'r '
if self.pub_C.get_num_connections()>0:
cmdOn+=' C'
else:
cmdOff+=' C'
if self.pub_L.get_num_connections()>0:
cmdOn+=' L'
else:
cmdOff+=' L'
if self.pub_R.get_num_connections()>0:
cmdOn+=' R'
else:
cmdOff+=' R'
if self.pub_O.get_num_connections()>0:
cmdOn+=' O'
else:
cmdOff+=' O'
if len(cmdOff)>2:
self.serial.addCommand(cmdOff)
if len(cmdOn)>2:
self.serial.addCommand(cmdOn)
lastSensorCommandSent = rospy.get_rostime().to_sec()
'''
data = self.serial.getData()
if data is not None:
self.pub_serial_out.publish(String(data))
if data.startswith('R '):
datas = string.split(data, ' ')
for i, reading in enumerate(datas):
reading_arr = string.split(reading, ':')
if i > 0 and len(reading_arr) == 2:
rType = reading_arr[0]
rVal = reading_arr[1]
if rType == 'L':
L.header.stamp = rospy.get_rostime()
L.header.seq += 1
L.range = int(rVal)/1000.0 if int(rVal) > 0 else NaN
self.pub_L.publish(L)
elif rType == 'R':
R.header.stamp = rospy.get_rostime()
R.header.seq += 1
R.range = int(rVal)/1000.0 if int(rVal) > 0 else NaN
self.pub_R.publish(R)
pass
elif rType == 'C':
C.header.stamp = rospy.get_rostime()
C.header.seq += 1
C.range = int(rVal)/1000.0 if int(rVal) > 0 else NaN
self.pub_C.publish(C)
elif rType == 'O':
rVal_arr = string.split(rVal, ',')
if len(rVal_arr) == 5:
O.header.stamp = rospy.get_rostime()
O.pose.pose.position.x = int(rVal_arr[0])/1000.0
O.pose.pose.position.y = int(rVal_arr[1])/1000.0
q = tf.transformations.quaternion_about_axis(int(rVal_arr[2])/1000.0, (0, 0, 1))
O.pose.pose.orientation.z = q[2]
O.pose.pose.orientation.w = q[3]
O.twist.twist.linear.x = int(rVal_arr[3])/1000.0
O.twist.twist.angular.z = int(rVal_arr[4])/1000.0
self.pub_O.publish(O)
self.pub_tf.sendTransform(
(O.pose.pose.position.x, O.pose.pose.position.y, O.pose.pose.position.z),
(O.pose.pose.orientation.x, O.pose.pose.orientation.y, O.pose.pose.orientation.z, O.pose.pose.orientation.w),
rospy.get_rostime(), 'base_link', 'odom')
r.sleep()
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)
time.sleep(0.2)
import rospy
from sensor_msgs.msg import Range
try:
rospy.init_node('sonar')
pub = rospy.Publisher('sonar',Range,queue_size=10)
ser = serial.Serial('/dev/cu.usbmodem1411',115200)
ser.flushInput()
t1 = threading.Thread(target=serial_reader)
t2 = threading.Thread(target=cmd_reader)
t1.start()
t2.start()
r_msg = Range()
r_msg.radiation_type = r_msg.ULTRASOUND
r_msg.field_of_view = 0.1
r_msg.min_range = 0.1
r_msg.max_range = 2.0
r_msg.header.frame_id = 'sonar_link'
r= rospy.Rate(20)
while not rospy.is_shutdown():
yaw = (90-servo_cmd)/180.0*3.14
br = tf.TransformBroadcaster()
br.sendTransform((0,0,0),
tf.transformations.quaternion_from_euler(0,0,yaw),
rospy.Time.now(),"world","sonar_link")
r_msg.range = d_cm/100.0
r_msg.header.stamp = rospy.Time.now()
import roslib; roslib.load_manifest('rviz')
from sensor_msgs.msg import Range
import rospy
topic = 'test_range'
publisher = rospy.Publisher(topic, Range)
rospy.init_node('range_test')
dist = 3
while not rospy.is_shutdown():
r = Range()
r.header.frame_id = "/moon"
r.header.stamp = rospy.Time.now()
r.radiation_type = 0
r.field_of_view = 2.0/dist
r.min_range = .4
r.max_range = 10
r.range = dist
publisher.publish( r )
rospy.sleep(0.1)
dist += .3
if dist > 10:
dist = 3
