def _hw_ultrasonics_cb(self, msg):
leftMsg = Range()
leftMsg.header.stamp = rospy.Time.now()
leftMsg.header.frame_id = "left_ultrasonic_link"
leftMsg.radiation_type = Range.ULTRASOUND
leftMsg.field_of_view = msg.field_of_view
leftMsg.min_range = msg.min_range
leftMsg.max_range = msg.max_range
leftMsg.range = msg.left_range
self._left_ultra_pub.publish(leftMsg)
centerMsg = Range()
centerMsg.header.stamp = rospy.Time.now()
centerMsg.header.frame_id = "center_ultrasonic_link"
centerMsg.radiation_type = Range.ULTRASOUND
centerMsg.field_of_view = msg.field_of_view
centerMsg.min_range = msg.min_range
centerMsg.max_range = msg.max_range
centerMsg.range = msg.center_range
self._center_ultra_pub.publish(centerMsg)
rightMsg = Range()
rightMsg.header.stamp = rospy.Time.now()
rightMsg.header.frame_id = "right_ultrasonic_link"
rightMsg.radiation_type = Range.ULTRASOUND
rightMsg.field_of_view = msg.field_of_view
rightMsg.min_range = msg.min_range
rightMsg.max_range = msg.max_range
rightMsg.range = msg.right_range
self._right_ultra_pub.publish(rightMsg)
def talker():
top_pub = rospy.Publisher('/IR_top', Range, queue_size=1)
left_pub = rospy.Publisher('/IR_left', Range, queue_size=1)
right_pub = rospy.Publisher('/IR_right', Range, queue_size=1)
rospy.init_node('IR_node', anonymous=True)
rate = rospy.Rate(6) # 10hz
#top_data
top_range = Range()
top_range.header.frame_id = '/IR_top'
top_range.field_of_view = 0.4
top_range.max_range = 3
top_range.min_range = 0.003
print('TOF start')
#left
left_range = Range()
left_range.header.frame_id = '/IR_left'
left_range.field_of_view = 0.4
left_range.max_range = 3
left_range.min_range = 0.005
#right
right_range = Range()
right_range.header.frame_id = '/IR_right'
right_range.field_of_view = 0.4
right_range.max_range = 3
right_range.min_range = 0.003
time.sleep(3)
while not rospy.is_shutdown():
receiced_distance = ir_pub()
#top
top_range.header.stamp = rospy.Time.now()
top_range.range = receiced_distance[0]
#print('top is %f \n' % top_range.range)
top_pub.publish(top_range)
#left
left_range.header.stamp = rospy.Time.now()
left_range.range = receiced_distance[12]
# print('left is %f \n' %left_range.range)
left_pub.publish(left_range)
#right
right_range.header.stamp = rospy.Time.now()
right_range.range = receiced_distance[4]
#print('right is %f \n' % right_range.range)
right_pub.publish(right_range)
rate.sleep()
def publish(self, data):
msg = Range()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = self._frameId
if self._urfType == URF_HRLV:
msg.min_range = MIN_RANGE_URF_HRLV_MaxSonar
msg.max_range = MAX_RANGE_URF_HRLV_MaxSonar
msg.field_of_view = FIELD_OF_VIEW_URF_HRLV_MaxSonar
else:
msg.min_range = MIN_RANGE_URF_LV_MaxSonar
msg.max_range = MAX_RANGE_URF_LV_MaxSonar
msg.field_of_view = FIELD_OF_VIEW_URF_LV_MaxSonar
msg.radiation_type = Range.ULTRASOUND
msg.range = data
self._pub.publish(msg)
def publish(self, data):
msg = Range()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = self._frameId
if self._urfType == URF_HRLV:
msg.min_range = MIN_RANGE_URF_HRLV_MaxSonar
msg.max_range = MAX_RANGE_URF_HRLV_MaxSonar
msg.field_of_view = FIELD_OF_VIEW_URF_HRLV_MaxSonar
else:
msg.min_range = MIN_RANGE_URF_LV_MaxSonar
msg.max_range = MAX_RANGE_URF_LV_MaxSonar
msg.field_of_view = FIELD_OF_VIEW_URF_LV_MaxSonar
msg.radiation_type = Range.ULTRASOUND
msg.range = data
self._pub.publish(msg)
def 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 __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 distance_thread():
global b
# ROS topic publisher for sonar distance readings
pub = rospy.Publisher('nxt/sonar', Range, queue_size=10)
# Sonar driver
ultrasonic = Ultrasonic(b, PORT_4)
# Setup the ROS range message with sensor properties
range_msg = Range()
range_msg.radiation_type = Range.ULTRASOUND
range_msg.field_of_view = 10.0 * pi / 180.0
range_msg.min_range = 0.01
range_msg.max_range = 2.55
# Rate limiter for process loop
rate = rospy.Rate(4)
# Main process loop
while not rospy.is_shutdown():
# Perform and read US range measurement
try:
d = ultrasonic.get_sample()
except:
rospy.logfatal("Connection with NXT robot interrupted!")
rospy.signal_shutdown("Connexion error")
# Fill and publish the ROS range message
range_msg.header.stamp = rospy.Time.now()
range_msg.range = 0.01 * d
pub.publish(range_msg)
# Wait until next loop
rate.sleep()
def main():
range_data = Range()
rospy.init_node('srf_08_rangefinder', anonymous=True)
device_address = rospy.get_param('~device_addr')
sample_rate = rospy.get_param('~sample_rate')
range_data.radiation_type = rospy.get_param('~radiation_type')
range_data.min_range = rospy.get_param('~min_range')
range_data.max_range = rospy.get_param('~max_range')
range_data.field_of_view = rospy.get_param('~fov')
pub = rospy.Publisher('rangefinder_data', Range, queue_size=1)
rate = rospy.Rate(sample_rate)
rangefinder = srf_08(device_address)
while not rospy.is_shutdown():
range_data.header.stamp = rospy.Time.now()
range_data.range = (rangefinder.distance() / 100)
pub.publish(range_data)
rate.sleep()
def 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 _publish_laser(self):
"""
TODO Griffin: Add a transform from base_link to base_laser
TODO: Modify the is valid condition if significant slope traversal object detection required.
Publish filtered laser distance as Range message. Will publish NaN distance if
scanner is blocked or robot in orientation not suited to horizontal navigation.
See API for sensor range, fov etc
https://developer.anki.com/vector/docs/generated/anki_vector.proximity.html
"""
if self._laser_pub.get_num_connections() == 0:
return
now = rospy.Time.now()
laser = Range()
laser.header.frame_id = self._base_frame
laser.header.stamp = now
laser.radiation_type = 1 # IR laser
laser.field_of_view = 0.436332 # 25 deg
laser.min_range = 0.03 # 30mm
laser.max_range = 1.5 # 300mm
laser_reading = self._vector.proximity.last_sensor_reading
if(laser_reading.is_valid):
laser.range = self._vector.proximity.last_sensor_reading.distance.distance_mm/1000
else:
laser.range = float('nan')
self._laser_pub.publish(laser)
def main():
rospy.init_node('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 timerCallback(self, event):
data = self.pinger.get_distance()
if data:
range_msg = Range()
range_msg.header.stamp = rospy.Time.now()
range_msg.header.frame_id = 'pinger'
range_msg.header.seq = self.count
range_msg.field_of_view = self.field_of_view
range_msg.min_range = 0.5
range_msg.max_range = 30.0
range_msg.range = float(data["distance"]) / 1000.0
self.range_pub.publish(range_msg)
ping_msg = PingRange()
ping_msg.header = range_msg.header
ping_msg.field_of_view = range_msg.field_of_view
ping_msg.min_range = range_msg.min_range
ping_msg.max_range = range_msg.max_range
ping_msg.range = range_msg.range
ping_msg.confidence = data['confidence'] / 100.0
self.ping_pub.publish(ping_msg)
else:
oar_msg = Bool()
oar_msg.data = True
self.out_of_range_pub.publish(oar_msg)
rospy.loginfo("Failed to get distance data")
self.count += 1
def __init__(self):
rospy.init_node('sonar', anonymous=True)
topic_name_d = '/sonarTP_D'
self.distance_publisher_down = rospy.Publisher(topic_name_d,
Range,
queue_size=5)
self.r = rospy.Rate(40)
self.serialPort0 = "/dev/ttyUSB0"
self.serialPort1 = "/dev/ttyUSB0"
self.serialPort2 = "/dev/ttyUSB0"
self.serialPort3 = "/dev/ttyUSB0"
self.serialPort4 = "/dev/ttyUSB0"
self.maxRange = 2000 # change for 5m vs 10m sensor
self.sleepTime = 0.1
self.minMM = 0.3
self.maxMM = 3
self.mm = float
r = Range()
r.header.stamp = rospy.Time.now()
r.header.frame_id = "/sonarD_link"
r.radiation_type = 0
r.field_of_view = 0.8
self.min_range = self.minMM
self.max_range = self.maxRange
r.min_range = self.min_range
r.max_range = self.max_range
self._range = r
def test_map_received(self):
poseMsg = PoseStamped()
poseMsg.header = Header()
poseMsg.pose.position.x = 0
poseMsg.pose.position.y = 0
poseMsg.pose.position.z = 0
poseMsg.pose.orientation.x = 0
poseMsg.pose.orientation.y = 0
poseMsg.pose.orientation.z = 0
poseMsg.pose.orientation.w = 1
self.pose_pub.publish(poseMsg)
rangeMsg = RangeArray()
rangeMsg.header = Header()
ranges = [0, 0.2, -1, 1, 1.5, 8, 3, 1, 8, 4, 2.3, 0.2, 0.1, 2, 3, 1, 9]
for rangerange in ranges:
range = Range()
range.header = Header()
range.range = rangerange
range.field_of_view = 1
range.min_range = 0.1
range.max_range = 2.1
rangeMsg.ranges.append(range)
rangeMsg.ranges = rangeMsg.ranges[:self.NUM_RANGE_SENSORS]
self.range_pub.publish(rangeMsg)
rospy.sleep(0.2)
self.assertEqual(self.rx.receivedMsg, True)
self.assertGreater(len(self.rx.map.points), 0)
def callDist(data):
#reads voltage value
voltage = ADC.read("P9_40")
#converts voltage values into distance(meters)
distance = (voltage**-.8271732796)
distance = distance*.1679936709
#checks and discards data outside of accurate range
if distance>2:
distance = 2
elif distance<.15:
distance = .15
#Writes data to Range message
msg = Range()
header = Header()
#creates header
msg.header.stamp.secs = rospy.get_time()
msg.header.frame_id = "/base_link"
msg.radiation_type = 1
msg.field_of_view = .15 #about 8.5 degrees
msg.min_range = .15
msg.max_range = 2
msg.range = distance
pub.publish(msg)
def 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 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 distance_callback(self, stamp):
distance_from_center = 0.035
for key in self.sensors:
msg = Range()
msg.field_of_view = self.sensors[key].getAperture()
msg.min_range = intensity_to_distance(
self.sensors[key].getMaxValue() - 8.2) + distance_from_center
msg.max_range = intensity_to_distance(
self.sensors[key].getMinValue() + 3.3) + distance_from_center
msg.range = intensity_to_distance(self.sensors[key].getValue())
msg.radiation_type = Range.INFRARED
self.sensor_publishers[key].publish(msg)
# Max range of ToF sensor is 2m so we put it as maximum laser range.
# Therefore, for all invalid ranges we put 0 so it get deleted by rviz
msg = LaserScan()
msg.header.frame_id = 'laser_scanner'
msg.header.stamp = stamp
msg.angle_min = 0.0
msg.angle_max = 2 * pi
msg.angle_increment = 15 * pi / 180.0
msg.scan_time = self.timestep.value / 1000
msg.range_min = intensity_to_distance(
self.sensors['ps0'].getMaxValue() - 20) + distance_from_center
msg.range_max = 1.0 + distance_from_center
msg.ranges = [
self.sensors['tof'].getValue(), # 0
intensity_to_distance(self.sensors['ps7'].getValue()), # 15
0.0, # 30
intensity_to_distance(self.sensors['ps6'].getValue()), # 45
0.0, # 60
0.0, # 75
intensity_to_distance(self.sensors['ps5'].getValue()), # 90
0.0, # 105
0.0, # 120
0.0, # 135
intensity_to_distance(self.sensors['ps4'].getValue()), # 150
0.0, # 165
0.0, # 180
0.0, # 195
intensity_to_distance(self.sensors['ps3'].getValue()), # 210
0.0, # 225
0.0, # 240
0.0, # 255
intensity_to_distance(self.sensors['ps2'].getValue()), # 270
0.0, # 285
0.0, # 300
intensity_to_distance(self.sensors['ps1'].getValue()), # 315
0.0, # 330
intensity_to_distance(self.sensors['ps0'].getValue()), # 345
self.sensors['tof'].getValue(), # 0
]
for i in range(len(msg.ranges)):
if msg.ranges[i] != 0:
msg.ranges[i] += distance_from_center
self.laser_publisher.publish(msg)
def altitude_pub(self, alt):
rng = Range()
rng.field_of_view = math.pi * 0.1
rng.max_range = 300
rng.header.frame_id = "sonar_link"
rng.header.stamp = rospy.Time.now()
rng.range = alt
return rng
def altitude_pub(self, alt):
rng = Range()
rng.field_of_view = math.pi * 0.1
rng.max_range = 300
rng.header.frame_id = "sonar_link"
rng.header.stamp = rospy.Time.now()
rng.range = alt
return rng
def 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 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 trigger(self):
ds = Range()
ds.header.frame_id = '/ultrasonic_link'
ds.header.stamp = rospy.Time.now()
ds.range = self.ultrasonic.get_sample() / 100.0
ds.field_of_view = self.spread
ds.min_range = self.min_range
ds.max_range = self.max_range
self.pub.publish(ds)
def trigger(self):
ds = Range()
ds.header.frame_id = '/ultrasonic_link'
ds.header.stamp = rospy.Time.now()
ds.range = self.ultrasonic.get_sample()/100.0
ds.field_of_view = self.spread
ds.min_range = self.min_range
ds.max_range = self.max_range
self.pub.publish(ds)
def 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 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 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 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 talker():
pub = rospy.Publisher('/IR_top', Range, queue_size=20)
left_pub = rospy.Publisher('/IR_left', Range, queue_size=20)
right_pub = rospy.Publisher('/IR_right', Range, queue_size=20)
rospy.init_node('IR_node', anonymous=True)
rate = rospy.Rate(10) # 5hz
r = Range()
r.header.frame_id = '/IR_top'
#r.radiation_type = Range.INFRARED
r.field_of_view = 0.21
r.max_range = 0.80
r.min_range = 0.10
left = Range()
left.header.frame_id = '/IR_left'
#left.radiation_type = Range.INFRARED
left.field_of_view = 0.21
left.max_range = 0.80
left.min_range = 0.10
right = Range()
right.header.frame_id = '/IR_right'
#right.radiation_type = Range.INFRARED
right.field_of_view = 0.21
right.max_range = 0.80
right.min_range = 0.1
print('IR start')
while not rospy.is_shutdown():
r.header.stamp = rospy.Time.now()
r.range = top_IR()
pub.publish(r)
left.header.stamp = rospy.Time.now()
left.range = left_IR()
left_pub.publish(left)
right.header.stamp = rospy.Time.now()
right.range = right_IR()
right_pub.publish(right)
rate.sleep()
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 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 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 pub_range(self):
rang_msg = Range()
for i in range(self.num_cam):
rang_msg.header.stamp = rospy.Time.now()
frame_id = "robot"+str(i+1)+"/camera_link"
rang_msg.header.frame_id = frame_id
rang_msg.field_of_view = self.field_of_view # 0-ULTRASOUND, 1-INFRARED
rang_msg.min_range = 0.1 # min_range of sonar, useless here
rang_msg.max_range = 201 # max_range of sonar, useless here
rang_msg.range = self.radius
self.pubs[i].publish(rang_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 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'):
""" Publish the data of the infrared sensor as a ROS Range message """
msg = Range()
msg.radiation_type = Range.INFRARED
msg.field_of_view = 20
msg.min_range = 0
msg.max_range = self.component_instance.bge_object['laser_range']
tmp = msg.max_range
for ray in self.data['range_list']:
if tmp > ray:
tmp = ray
msg.range = tmp
self.publish(msg)
def default(self, ci='unused'):
msg = Range()
msg.header = self.get_ros_header()
msg.radiation_type = Range.INFRARED
msg.field_of_view = 20
msg.min_range = 0
msg.max_range = self.component_instance.bge_object['laser_range']
tmp = msg.max_range
for ray in self.data['range_list']:
if tmp > ray:
tmp = ray
msg.range = tmp
self.publish(msg)
def inputCallback(self, msg):
#########################################################################
# rospy.loginfo("-D- range_filter inputCallback")
cur_val = msg.value
if cur_val <= self.max_valid and cur_val >= self.min_valid:
self.prev.append(cur_val)
del self.prev[0]
p = array(self.prev)
self.rolling_ave = p.mean()
self.rolling_std = p.std()
self.rolling_meters = ((self.b * self.rolling_ave) ** self.m) / 100
self.filtered_pub.publish( self.rolling_meters )
self.std_pub.publish( self.rolling_std )
rng = Range()
rng.radiation_type = 1
rng.min_range = self.min_range
rng.max_range = self.max_range
rng.range = self.rolling_meters
rng.header.frame_id = self.frame
rng.field_of_view = 0.1
rng.header.stamp = rospy.Time.now()
self.range_pub.publish( rng )
ranges = []
intensities = []
angle_start = 0.0 - self.field_of_view
angle_stop = self.field_of_view
for angle in linspace( angle_start, angle_stop, 10):
ranges.append( self.rolling_meters )
intensities.append( 1.0 )
scan = LaserScan()
scan.ranges = ranges
scan.header.frame_id = self.frame
scan.time_increment = 0;
scan.range_min = self.min_range
scan.range_max = self.max_range
scan.angle_min = angle_start
scan.angle_max = angle_stop
scan.angle_increment = (angle_stop - angle_start) / 10
scan.intensities = intensities
scan.scan_time = self.scan_time
scan.header.stamp = rospy.Time.now()
self.scan_pub.publish( scan )
def getDistance(self):
while True:
self.dist=input("plase input dis:")
try:
print "you have input:"+str(self.dist)
except:
print "you have input nothing"
ran = Range()
ran.header.stamp = rospy.Time.now()
ran.header.frame_id = "/ultrasound"
ran.field_of_view = 1;
ran.min_range = 0;
ran.max_range = 5;
ran.range = self.dist;
self.ultrasound_pub.publish(ran)
def 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 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 publish(self):
ts = rospy.Time.now()
self.tf_broadcaster.sendTransform(self.position, self.orientation,
ts, self.frame_id, 'odom')
msg = Range()
msg.header.stamp = ts
msg.header.frame_id = self.frame_id
msg.field_of_view = self.fov
msg.max_range = self.max_range
if self.caching and self.cache:
msg.range = self.cache
else:
try:
distance = self.nearest_point().distances[0]
if distance > self.max_range:
msg.range = self.max_range
else:
msg.range = distance
except rospy.ServiceException:
return
self.cache = msg.range
self.range_pub.publish(msg)
def 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)
#!/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()
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
#!/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.),
# 0.025, frame = '/camera_link',
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()
pub.publish(r_msg)
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)
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():
#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)
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
