def laserscan_err_inj(tb3_name):
#Create error-injected topic
rospy.init_node('laser_err_inj')
#########################################
#Create new message
laserscan_msg = LaserScan()
#Fill message with values
laserscan_msg.header.seq = 0
laserscan_msg.header.stamp.secs = 0
laserscan_msg.header.stamp.nsecs = 0
laserscan_msg.header.frame_id = ""
laserscan_msg.angle_min = 0.0
laserscan_msg.angle_max = 0.0
laserscan_msg.angle_increment = 0.0
laserscan_msg.time_increment = 0.0
laserscan_msg.scan_time = 0.0
laserscan_msg.range_min = 0.0
laserscan_msg.range_max = 0.0
laserscan_msg.ranges = [0.0] * 360
laserscan_msg.intensities = [0.0] * 360
#########################################
rate = rospy.Rate(50)
#Publish message into new topic
while not rospy.is_shutdown():
my_sub = rospy.Subscriber(turtlebot_dict[tb3_name] + 'scan', LaserScan, listener)
my_pub = rospy.Publisher(turtlebot_dict[tb3_name] + 'laser_err_inj', LaserScan, queue_size = 10)
#########################################
#INJECT ERRORS HERE
laserscan_msg.header.seq = actual_seq
laserscan_msg.header.stamp.secs = actual_secs
laserscan_msg.header.stamp.nsecs = actual_nsecs
laserscan_msg.header.frame_id = actual_frameid
laserscan_msg.angle_min = actual_anglemin
laserscan_msg.angle_max = actual_anglemax
laserscan_msg.angle_increment = actual_angleincrement
laserscan_msg.time_increment = actual_timeincrement
laserscan_msg.scan_time = actual_scantime
laserscan_msg.range_min = actual_rangemin
laserscan_msg.range_max = actual_rangemax
for i in range(len(actual_ranges)):
laserscan_msg.ranges[i] = actual_ranges[i] #inject error here (i just made everything = 0 because it's easy to see when testing)
for j in range(len(actual_intensities)):
laserscan_msg.intensities[i] = actual_intensities[i] #inject error here (i just made everything = 0 because it's easy to see when testing)
#########################################
my_pub.publish(laserscan_msg)
rate.sleep()
rospy.spin()
def mapping(self, ScanData): # remap data from SBG_IMU_msg to ROS_IMU_msg and publish FakeScan = LaserScan() # FakeScan = ScanData FakeScan.header.frame_id = 'laser' FakeScan.header.seq = ScanData.header.seq FakeScan.header.stamp = ScanData.header.stamp FakeScan.angle_min = ScanData.angle_min FakeScan.angle_max = ScanData.angle_max FakeScan.angle_increment = ScanData.angle_increment FakeScan.time_increment = ScanData.time_increment FakeScan.scan_time = ScanData.scan_time FakeScan.range_min = ScanData.range_min FakeScan.range_max = ScanData.range_max i = 0 FakeScan.ranges = [1] * len(ScanData.ranges) FakeScan.intensities = [1] * len(ScanData.intensities) for i in range(len(ScanData.ranges)): if ScanData.ranges[i] < ScanData.range_min: FakeScan.ranges[i] = 30.0 FakeScan.intensities[i] = 430.0 else: FakeScan.ranges[i] = ScanData.ranges[i] FakeScan.intensities[i] = ScanData.intensities[i] self.pub.publish(FakeScan)
def laser_callback(self, msg):
filtered_values = LaserScan()
distance = np.array(msg.ranges)
filtered_values.header = msg.header
filtered_values.angle_increment = msg.angle_increment
filtered_values.time_increment = msg.time_increment
filtered_values.scan_time = msg.scan_time
filtered_values.range_min = msg.range_min
filtered_values.range_max = msg.range_max
filtered_values.intensities = msg.intensities
angle = filtered_values.angle_increment
min_angle = msg.angle_min
max_angle = msg.angle_max
laser_noise_variance = rospy.get_param('laser_noise_variance')
if laser_noise_variance <= 0:
laser_noise_variance = 0.1
filtered_values_ranges = np.zeros(len(distance))
noise_values_ranges = np.random.normal(loc=0,
scale=laser_noise_variance,
size=len(distance))
for i in range(len(distance)):
filtered_values_ranges[i] = noise_values_ranges[i] + distance[i]
filtered_values.ranges = filtered_values_ranges
filtered_values.angle_min = min_angle
filtered_values.angle_max = max_angle
self.scan_pub.publish(filtered_values)
def main():
rospy.init_node('fake_scan', anonymous=True)
scan_subscriber = rospy.Subscriber('/remapped_scan', LaserScan,
scan_callback)
scan_publisher = rospy.Publisher('/scan', LaserScan, queue_size=10)
rate = rospy.Rate(3)
m = 720
empty_scan_msg = LaserScan()
empty_scan_msg.angle_min = -2.3561899662
empty_scan_msg.angle_max = 2.3561899662
empty_scan_msg.angle_increment = 0.0065540750511
empty_scan_msg.time_increment = 0.0
empty_scan_msg.scan_time = 0.0
empty_scan_msg.range_min = 0.10000000149
empty_scan_msg.range_max = 30.0
empty_scan_msg.ranges = [0.0 for i in range(m)]
empty_scan_msg.intensities = [float('inf') for i in range(m)]
h = std_msgs.msg.Header()
h.frame_id = "base_laser"
for i in range(10):
h.stamp = rospy.Time.now()
h.seq += 1
empty_scan_msg.header = h
scan_publisher.publish(empty_scan_msg)
rate.sleep()
#infinite range, zero intensity --> can't detect anyting
empty_scan_msg.ranges = [float('inf') for i in range(m)]
empty_scan_msg.intensities = [0.0 for i in range(m)]
while not rospy.is_shutdown():
h.stamp = rospy.Time.now()
h.seq += 1
empty_scan_msg.header = h
scan_publisher.publish(empty_scan_msg)
rate.sleep()
def create_lidar_msg(self, L):
raw_lidar = L.data
#stripped_lidar = raw_lidar.translate(None, '[]').translate(None, '"').translate(None, '\'')
array_lidar = raw_lidar.split(";")
lidar_msg = LaserScan()
lidar_msg.header = self.create_header() #self?
lidar_msg.angle_min = math.radians(float(array_lidar[0]))
lidar_msg.angle_max = math.radians(float(array_lidar[1]))
lidar_msg.angle_increment = math.radians(0.25) #MAKE PARAM
lidar_msg.time_increment = 0.025 / (
270 * 4) #time in ms / measurements YOYOYOYO CHECK THIS
lidar_msg.scan_time = float(array_lidar[2]) / 1000 #time in ms
lidar_msg.range_min = float(
array_lidar[4]) / 1000 #sent in mm, should be meters
lidar_msg.range_max = float(
array_lidar[5]) / 1000 #sent in mm, should be meters
array_string = array_lidar[3] #.translate(None, '[]')
string_array = array_string.split(",")
lidar_msg.ranges = [float(r) / 1000
for r in string_array] #better way?
self.scanPub.publish(lidar_msg)
def LaserReceiver():
pub = rospy.Publisher('scanPi', LaserScan, queue_size=10)
rospy.init_node('LaserReceiver', anonymous=True)
rate = rospy.Rate(10) # 10hz
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP
sock.bind(("192.168.1.65", 4001))
while not rospy.is_shutdown():
data, addr = sock.recvfrom(15024)
data = json.loads(data)
laser = LaserScan()
laser.header.stamp.secs = data["header"]["stamp"]["secs"]
laser.header.stamp.nsecs = data["header"]["stamp"]["nsecs"]
laser.header.frame_id = data["header"]["frame_id"]
laser.header.seq = data["header"]["seq"]
laser.angle_min = data["angle_min"]
laser.angle_max = data["angle_max"]
laser.angle_increment = data["angle_increment"]
laser.time_increment = data["time_increment"]
laser.range_min = data["range_min"]
laser.range_max = data["range_max"]
laser.scan_time = data["scan_time"]
laser.ranges = data["ranges"]
laser.intensities = data["intensities"]
pub.publish(laser)
rate.sleep()
def scanner():
pub_topic = rospy.get_param("/fake_scan_publisher/pub_topic")
pub = rospy.Publisher(pub_topic, LaserScan, queue_size=10)
pub_rate = rospy.get_param("/fake_scan_publisher/pub_rate")
rate = rospy.Rate(pub_rate) # default 20 hz
while not rospy.is_shutdown():
scan = LaserScan()
scan.header.stamp = rospy.Time.now()
scan.header.frame_id = "base_link"
scan.angle_min = rospy.get_param(
"/fake_scan_publisher/angle_min") # -(2.0/3.0)*math.pi
scan.angle_max = rospy.get_param(
"/fake_scan_publisher/angle_max") # (2.0/3.0)*math.pi
scan.angle_increment = rospy.get_param(
"/fake_scan_publisher/angle_increment") # (1/300.0)*math.pi
scan.scan_time = 0.05
scan.range_min = rospy.get_param(
"/fake_scan_publisher/range_min") # 1.0
scan.range_max = rospy.get_param(
"/fake_scan_publisher/range_max") # 10.0
for i in range(
int((scan.angle_max - scan.angle_min) / scan.angle_increment +
1)):
scan.ranges.append(uniform(scan.range_min, scan.range_max))
rospy.loginfo(scan)
pub.publish(scan)
rate.sleep()
def create_lidar_msg(lidar_string):
lidar_msg = LaserScan()
data = lidar_string.split(";")
#num_readings = 1440 --------------------------------
#data[0] = min angle (degrees)
#data[1] = max angle (degrees)
#data[2] = timestep (ms)
#data[3] = lidar scan array
#data[4] = min range
#data[5] = max range
#print data
lidar_msg.header = create_header() #self?
lidar_msg.angle_min = math.radians(float(data[0]))
lidar_msg.angle_max = math.radians(float(data[1]))
lidar_msg.angle_increment = math.radians(.25) #get from lidar
lidar_msg.time_increment = float(25. / self.num_readings) #time in ms / measurements YOYOYOYO CHECK THIS
lidar_msg.scan_time = float(data[2])
lidar_msg.range_min = float(data[4]) / 1000 #sent in mm, should be meters
lidar_msg.range_max = float(data[5]) / 1000 #sent in mm, should be meters
array_string = data[3].translate(None, '[]')
string_array = array_string.split(",")
lidar_msg.ranges = [float(r) / 1000 for r in string_array] #better way?
# string_array = data[3].strip("[").strip("]").split(",")
# string_array = data[3].split(",")
# try:
# lidar_msg.ranges = [float(r) for r in string_array]
# lidar_msg.intensities = []
# except ValueError:
# print "range vals failed"
return lidar_msg
def data_fusion(asus_data, laser_data, LaserData, transform):
if asus_data != None and laser_data != None:
data = LaserScan()
data.angle_min = laser_data.angle_min
data.angle_max = laser_data.angle_max
data.header = laser_data.header
data.time_increment = laser_data.time_increment
data.angle_increment = laser_data.angle_increment
data.scan_time = laser_data.scan_time
data.range_min = laser_data.range_min
data.range_max = laser_data.range_max
data.ranges = []
angle = laser_data.angle_min
for i in laser_data.ranges:
if -0.510 <= angle and angle <= 0.510:
num = asus_data.angle_max
for j in asus_data.ranges:
if abs(num - angle) <= asus_data.angle_increment:
if i > j:
i = j
num -= asus_data.angle_increment
data.ranges.append(i)
angle -= laser_data.angle_increment
if len(data.ranges) == len(laser_data.ranges):
LaserData.append(data)
else:
if asus_data == None:
rospy.loginfo('wait for asus data')
if laser_data == None:
rospy.loginfo('wait for rplidar data')
def publish_filtered_laser_scan(self, laser_original_data, new_filtered_laser_range):
rospy.logdebug("new_filtered_laser_range==>" + str(new_filtered_laser_range))
laser_filtered_object = LaserScan()
h = Header()
h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
h.frame_id = laser_original_data.header.frame_id
laser_filtered_object.header = h
laser_filtered_object.angle_min = laser_original_data.angle_min
laser_filtered_object.angle_max = laser_original_data.angle_max
new_angle_incr = abs(laser_original_data.angle_max - laser_original_data.angle_min) / len(
new_filtered_laser_range)
# laser_filtered_object.angle_increment = laser_original_data.angle_increment
laser_filtered_object.angle_increment = new_angle_incr
laser_filtered_object.time_increment = laser_original_data.time_increment
laser_filtered_object.scan_time = laser_original_data.scan_time
laser_filtered_object.range_min = laser_original_data.range_min
laser_filtered_object.range_max = laser_original_data.range_max
laser_filtered_object.ranges = []
laser_filtered_object.intensities = []
for item in new_filtered_laser_range:
if item == 0.0:
laser_distance = 0.1
else:
laser_distance = item
laser_filtered_object.ranges.append(laser_distance)
laser_filtered_object.intensities.append(item)
self.laser_filtered_pub.publish(laser_filtered_object)
def data(self,a):
angle = []
for i in reversed(range(90)):
angle.append(i)
for i in range(270,360):
angle.append(i)
Range = [a.ranges[i] for i in angle]
# rospy.loginfo_throttle(2,a.ranges[0])
pub=rospy.Publisher('/CloudPoint',LaserScan,queue_size=10)
msg=LaserScan()
msg.header=a.header
msg.header.frame_id = 'laser'
msg.angle_min=a.angle_min
msg.angle_max=a.angle_max
msg.angle_increment=a.angle_increment
msg.time_increment=a.time_increment
msg.scan_time=a.scan_time
msg.range_min=a.range_min
msg.range_max=a.range_max
List = self.ranges
New = numpy.hstack([List,Range])
self.ranges = New
if len(self.ranges) >=4500:
self.ranges = self.ranges[900:]
msg.ranges=self.ranges
msg.intensities=a.intensities
pub.publish(msg)
print len(msg.ranges)
def publish_filtered_laser_scan(self, laser_original_data,
new_filtered_laser_range):
length_range = len(laser_original_data.ranges)
length_intensities = len(laser_original_data.intensities)
laser_filtered_object = LaserScan()
h = Header()
h.stamp = rospy.Time.now(
) # Note you need to call rospy.init_node() before this will work
h.frame_id = "chassis"
laser_filtered_object.header = h
laser_filtered_object.angle_min = laser_original_data.angle_min
laser_filtered_object.angle_max = laser_original_data.angle_max
laser_filtered_object.angle_increment = laser_original_data.angle_increment
laser_filtered_object.time_increment = laser_original_data.time_increment
laser_filtered_object.scan_time = laser_original_data.scan_time
laser_filtered_object.range_min = laser_original_data.range_min
laser_filtered_object.range_max = laser_original_data.range_max
laser_filtered_object.ranges = []
laser_filtered_object.intensities = []
for item in new_filtered_laser_range:
laser_filtered_object.ranges.append(item)
laser_filtered_object.intensities.append(item)
self.laser_filtered_pub.publish(laser_filtered_object)
def callback(self, msg):
rate = rospy.Rate(10)
scan_msg = LaserScan()
scan_msg.header = msg.header
scan_msg.angle_min = msg.angle_min
scan_msg.angle_max = msg.angle_max
scan_msg.angle_increment = msg.angle_increment
scan_msg.time_increment = msg.time_increment
scan_msg.scan_time = msg.scan_time
scan_msg.range_min = msg.range_min
scan_msg.range_max = msg.range_max
#print np.shape(msg.ranges)
count = int(msg.scan_time / msg.time_increment)
scan_msg.ranges = np.zeros((count, ), dtype=np.float)
scan_msg.intensities = np.zeros((count, ), dtype=np.float)
for i in xrange(count):
#if i < 359 or i > 1079:
if i < 489 or i > 949:
scan_msg.ranges[i] = msg.ranges[i]
scan_msg.intensities[i] = msg.intensities[i]
else:
scan_msg.ranges[i] = float('inf')
scan_msg.intensities[i] = 0.0
#print "here"
#rate.sleep()
self.scan_pub.publish(scan_msg)
def fake_scan_publisher():
rospy.init_node('fake_scan_publisher')
pub = rospy.Publisher(rospy.get_param("~pubs_topic", "fake_scan"),
LaserScan,
queue_size=10)
rate = rospy.Rate(rospy.get_param("~pubs_rate", 20)) # 20hz
while not rospy.is_shutdown():
current_time = rospy.Time.now()
scan = LaserScan()
scan.header.stamp = current_time
scan.header.frame_id = "base_link"
scan.angle_min = rospy.get_param("~angle_min", -2 * math.pi / 3)
scan.angle_max = rospy.get_param("~angle_max", 2 * math.pi / 3)
scan.angle_increment = rospy.get_param("~angle_increment",
math.pi / 300)
scan.scan_time = 1.0 / 20
scan.range_min = rospy.get_param("~range_min", 1.0)
scan.range_max = rospy.get_param("~range_max", 10.0)
num_readings = int(
(scan.angle_max - scan.angle_min) // scan.angle_increment) + 1
scan.ranges = [0] * num_readings
for i in range(0, num_readings):
scan.ranges[i] = random.uniform(scan.range_min, scan.range_max)
#rospy.loginfo(scan)
pub.publish(scan)
rate.sleep()
def merge_scans(rf, sg):
rf.ranges = list(rf.ranges)
for i in range(40):
rf.ranges[len(rf.ranges) - i - 1] = 0
if not sg:
rf.header.frame_id = 'laser'
return rf
else:
global angle_min
global angle_max
global angle_increment
global last_scan_time
if not last_scan_time:
last_scan_time = time.time()
scan = LaserScan()
scan.header.frame_id = 'laser'
scan.header.stamp = get_most_recent_timestamp(rf, sg)
scan.angle_min = angle_min
scan.angle_max = angle_max
scan.angle_increment = angle_increment
scan.scan_time = time.time() - last_scan_time
scan.time_increment = scan.scan_time / 541
scan.range_min = rf.range_min
scan.range_max = rf.range_max
scan.ranges = rf.ranges
for i in range(180 * 2):
if sg.ranges[i] < scan.ranges[90 + i] or scan.ranges[90 + i] == 0:
scan.ranges[90 + i] = sg.ranges[i]
return scan
def checker(fake_laser_param, realtime_lasers, nonrealtime_lasers):
r = rospy.Rate(RATE)
seq = 0
laser_scan = LaserScan()
laser_scan.header.seq = seq
laser_scan.header.frame_id = fake_laser_param['frame_name']
laser_scan.angle_min = fake_laser_param['angle_min']
laser_scan.angle_max = fake_laser_param['angle_max']
laser_scan.angle_increment = fake_laser_param['angle_increment']
laser_scan.range_min = fake_laser_param['range_min']
laser_scan.range_max = fake_laser_param['range_max']
laser_scan.scan_time = 0
laser_scan.time_increment = 0
pub = rospy.Publisher('/scan', LaserScan, queue_size=10)
while not rospy.is_shutdown():
fake_laser_data = realtime_lasers[0].get_range_data()
for laser_scanner in realtime_lasers[1:]:
new_laser_data = laser_scanner.get_range_data()
fake_laser_data = [min(r1, r2) for r1, r2 in zip(fake_laser_data, new_laser_data)]
for laser_scanner in nonrealtime_lasers:
laser_data = laser_scanner.get_range_data()
#fake_laser_data = [r1 if r1 < 1000 else min(r1, r2) for r1, r2 in zip(fake_laser_data, laser_data)]
fake_laser_data = [min(r1, r2) for r1, r2 in zip(fake_laser_data, laser_data)]
laser_scan.ranges = fake_laser_data
laser_scan.header.stamp = rospy.Time.now()
pub.publish(laser_scan)
seq += 1
r.sleep()
def talker():
pub = rospy.Publisher('recorded_scan', LaserScan, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(12)
lines = f.readlines()
scancount = 0
i=0
while not rospy.is_shutdown():
if i >= len(lines):
scancount = 0
i=0
f.seek(0)
if len(lines[i]) > 100:
scancount += 1
scan = LaserScan()
scan.header.stamp = rospy.Time.now()
scan.header.frame_id = 'laser'
scan.angle_min = -2.28987193108
scan.angle_max = 2.28987193108
scan.angle_increment = 0.0069813169539
scan.time_increment = 0.083333
scan.scan_time = 0.0833*scancount
scan.range_min = 0.002
scan.range_max = 50
scan.ranges = map(float, lines[i][1:-2].split(', '))
scan.intensities = map(float, lines[i][1:-2].split(', '))
pub.publish(scan)
rate.sleep()
i+=1
def auto_build_scan(tf_list,
rng_array,
number_of_pos,
offsets,
scan_frame,
scan_time=1. / 100):
corrected_range_array = [float('inf')] * number_of_pos
number_of_sensor = 8 # The parser outputs 8 values anyway
scan = LaserScan()
scan.scan_time = scan_time
scan.header.frame_id = scan_frame
scan.angle_max = 2 * math.pi
scan.angle_min = 0.0
scan.angle_increment = 2.0 * math.pi / number_of_pos
scan.time_increment = scan_time / number_of_pos
scan.range_min = rng_array.ranges[0].min_range
scan.range_max = rng_array.ranges[0].max_range
for i in range(number_of_sensor):
if tf_list[i] is not None:
yaw = tf_to_z(tf_list[i].transform)
yaw = wrap_to_2pi(yaw)
new_pos = int(round(yaw / (2.0 * math.pi) * number_of_pos))
if rng_array.ranges[i].range < 0:
pass # Out of range values (see parser for more info)
else:
corrected_range_array[
new_pos] = rng_array.ranges[i].range + offsets[i]
scan.ranges = corrected_range_array
scan.header.stamp = rng_array.header.stamp
return scan
def laser_test():
pub = rospy.Publisher('laser_scan', LaserScan)
rospy.init_node('laser_test')
laser_msg = LaserScan()
laser_msg.header.frame_id = 'laser'
laser_msg.angle_min = -1.5
laser_msg.angle_max = 1.5
laser_msg.angle_increment = 0.1
laser_msg.time_increment = 0.1
laser_msg.scan_time = 0.1
laser_msg.range_min = 0.5
laser_msg.range_max = 1.5
laser_msg.ranges = [
1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 9.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
1.0, 0.1, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
5.0, 1.0
]
laser_msg.intensities = laser_msg.ranges
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
laser_msg.header.stamp = rospy.get_rostime()
pub.publish(laser_msg)
r.sleep()
def scanCB(self, msg):
range_list = list(msg.ranges)
intensity_list = list(msg.intensities)
for i in xrange(len(msg.ranges)):
current_angle = msg.angle_min + msg.angle_increment * i
# Delete the first element
if (current_angle < np.deg2rad(self.ang_min)):
del range_list[0]
del intensity_list[0]
# Delete the last element
elif (current_angle > np.deg2rad(self.ang_max)):
del range_list[-1]
del intensity_list[-1]
new_scan = LaserScan()
new_scan.header = msg.header
new_scan.angle_min = np.deg2rad(self.ang_min)
new_scan.angle_max = np.deg2rad(self.ang_max)
new_scan.angle_increment = msg.angle_increment
new_scan.time_increment = msg.time_increment
new_scan.scan_time = msg.scan_time
new_scan.range_min = msg.range_min
new_scan.range_max = msg.range_max
new_scan.ranges = range_list
if (self.intensity == False):
new_scan.intensities = []
else:
new_scan.intensities = intensity_list
self.scan_pub.publish(new_scan)
def scanCallback(self, scan):
rmin = [0 , 315]
rmax = [45, 360]
reduced_scan = LaserScan()
reduced_scan.header = scan.header
reduced_scan.angle_min = scan.angle_min
reduced_scan.angle_max = scan.angle_max
reduced_scan.angle_increment = scan.angle_increment
reduced_scan.time_increment = scan.time_increment
reduced_scan.scan_time = scan.scan_time
reduced_scan.range_min = scan.range_min
reduced_scan.range_max = scan.range_max
reduced_scan.intensities = scan.intensities
for i in range(len(scan.ranges)):
if i >= rmin[0] and i <= rmax[0]-1:
reduced_scan.ranges.append(scan.ranges[i])
elif i >= rmin[1] and i <= rmax[1]-1:
reduced_scan.ranges.append(scan.ranges[i])
else:
reduced_scan.ranges.append(0)
rospy.loginfo("val[%d]:%f", i, reduced_scan.ranges[i])
self.pub_reduced_scan.publish(reduced_scan)
def talker():
ls= LaserScan()
ls.header.frame_id = 'map'
inc=0.02
amin=-pi/2 + fov*pi/180 #20 implies 70 deg field of view in one quadrant
amax=-float(amin)
ls.angle_increment=inc
ls.angle_max=amax
ls.angle_min=amin
ls.time_increment=0.0001
ls.scan_time=0.00001
ls.range_min=0.5
ls.range_max=4.0
pub=rospy.Publisher('laser_scan_test', LaserScan,queue_size=10)
rospy.init_node('lane_laser_scan', anonymous=True)
rate = rospy.Rate(10)
#cap = cv2.VideoCapture(os.path.dirname(__file__)+"/test_480.mp4")
cap = cv2.VideoCapture(1)
#cap.set(cv2.cv.CV_CAP_PROP_FPS, 60)
ret,frame=cap.read()
while ret:
ret,frame=cap.read()
ls.ranges=laserScan(frame,xc,yc,inc,amin,amax)
ls.header.stamp.secs=rospy.get_rostime().secs
ls.header.stamp.nsecs=rospy.get_rostime().nsecs
#ls.header.stamp.sec=rospy.Time
rospy.loginfo(str(ls))
pub.publish(ls)
rate.sleep()
def create_lidar_msg(L):
raw_lidar = L.data
stripped_lidar = raw_lidar.translate(None, '[]').translate(None,
'"').translate(
None, '\'')
array_lidar = stripped_lidar.split(",")
num_readings = 1440
scan = LaserScan()
scan.header.stamp = rospy.Time.now()
scan.header.frame_id = "base_scan"
scan.angle_min = math.radians(float(array_lidar[0]))
scan.angle_max = math.radians(float(array_lidar[1]))
scan.angle_increment = math.radians(.25) #get from lidar
scan.time_increment = float(25. / num_readings) #time in ms / measurements
scan.scan_time = float(array_lidar[2])
scan.range_min = float(array_lidar[4]) / 1000 #sent in mm, needs m
scan.range_max = float(array_lidar[5]) / 1000 #sent in mm, needs m
# string_array = array_lidar[3].strip("[").strip("]").split(",")
array_string = array_lidar[3].translate(None, '[]')
string_array = array_string.split(",")
scan.ranges = [float(r) / 1000 for r in string_array] #better way?
scanPub.publish(scan)
def callback(data):
laser_angular_range = 100
laser = LaserScan()
print type(data)
middle_index = int(
(data.angle_max - data.angle_min) / data.angle_increment) / 2
angle_increment_degree = data.angle_increment * (180 / PI)
print 'middle_index ', middle_index
print 'angle_increment_degree ', angle_increment_degree
for i in range(int(laser_angular_range / angle_increment_degree)):
laser.ranges.append(
data.ranges[middle_index -
int(laser_angular_range / angle_increment_degree / 2) +
i])
#laser.intensities.append(data.intensities[middle_index-int(laser_angular_range/angle_increment_degree/2)+i])
####################################################
laser.header = data.header
laser.header.frame_id = 'laser'
laser.angle_min = -laser_angular_range / 2 * (PI / 180)
laser.angle_max = laser_angular_range / 2 * (PI / 180)
laser.angle_increment = data.angle_increment
laser.time_increment = data.time_increment
laser.scan_time = data.scan_time
laser.range_min = data.range_min
laser.range_max = data.range_max
print laser.angle_increment
print(laser.angle_max - laser.angle_min) / laser.angle_increment - len(
laser.ranges)
#####################################################
print time.time()
laser_pub.publish(laser)
def merge_scans(rf, sg):
rf.ranges = list(rf.ranges)
for i in range(40):
rf.ranges[len(rf.ranges)-i-1] = 0
if not sg:
rf.header.frame_id = 'laser'
return rf
else:
global angle_min
global angle_max
global angle_increment
global last_scan_time
if not last_scan_time:
last_scan_time = time.time()
scan = LaserScan()
scan.header.frame_id = 'laser'
scan.header.stamp = get_most_recent_timestamp(rf, sg)
scan.angle_min = angle_min
scan.angle_max = angle_max
scan.angle_increment = angle_increment
scan.scan_time = time.time() - last_scan_time
scan.time_increment = scan.scan_time / 541
scan.range_min = rf.range_min
scan.range_max = rf.range_max
scan.ranges = rf.ranges
for i in range(180*2):
if sg.ranges[i] < scan.ranges[90 + i] or scan.ranges[90 + i] == 0:
scan.ranges[90 + i] = sg.ranges[i]
return scan
def main():
rospy.init_node('VL53L0X_node', anonymous=True)
timing = tof.get_timing()
while not rospy.is_shutdown():
current_time = rospy.Time.now()
distance = tof.get_distance() # Nuskaityti duomenis
rospy.loginfo("dist={}".format(distance))
# Atmesti negerus duomenis
if distance > 3000 or distance < 0:
distance = 0
# Headeris
laser_scan = LaserScan()
laser_scan.header.stamp = current_time
laser_scan.header.frame_id = "radar"
laser_scan.angle_min = -0.15
laser_scan.angle_max = 0.15
laser_scan.angle_increment = 0.05
laser_scan.time_increment = 0
laser_scan.scan_time = timing
laser_scan.range_min = 0.030
laser_scan.range_max = 2.500
laser_scan.ranges = [float32(distance / 1000)] * 7
pub_scan.publish(laser_scan)
rospy.Rate(1 / (timing / 1000000.00)).sleep()
def _processLaserscan(self, readingType, remappedTimestamp, content):
# FIXME: For type ROBOTLASER, we should publish the robot/sensor pose using TF and use the correct TF frame
laserscan = LaserScan()
laserscan.header = Header(stamp=remappedTimestamp, frame_id="odom", seq=self._laserscanCounter)
if readingType.startswith("RAWLASER") or readingType.startswith("ROBOTLASER"):
laserscan.angle_min = float(content[1])
laserscan.angle_max = laserscan.angle_min + float(content[2])
laserscan.angle_increment = float(content[3])
laserscan.time_increment = 0
laserscan.scan_time = 0.0 # FIXME
laserscan.range_min = 0
laserscan.range_max = float(content[4])
numRanges = int(content[7])
for i in xrange(0, numRanges):
laserscan.ranges.append( float(content[8 + i]) )
numRemissions = int(content[8 + numRanges])
for i in xrange(0, numRemissions):
laserscan.intensities.append( float(content[9 + numRanges + i]) )
else:
rospy.logwarn("Unsupported laser of type %s in line %d" % (readingType, self._lineCounter) )
publisher = self._laserscanPublishers[ self._getLaserID(readingType, content) ]
publisher.publish(laserscan)
self._laserscanCounter += 1
def handleScanMsg(data):
rospy.loginfo("\n/scan is now being handled...")
#### Setup Corrected-LaserScan Publisher
correctedScan_publisher = rospy.Publisher("/scanCorrected",
LaserScan,
queue_size=10)
correctedScan_msg = LaserScan()
#### Start copying the variables
correctedScan_msg.angle_min = data.angle_min
correctedScan_msg.angle_max = data.angle_max
correctedScan_msg.angle_increment = data.angle_increment
correctedScan_msg.time_increment = data.time_increment
correctedScan_msg.scan_time = data.scan_time
correctedScan_msg.range_min = data.range_min
correctedScan_msg.range_max = data.range_max
correctedScan_msg.intensities = data.intensities
#### Start copying 'ranges' using the corrected values if necessary
# if (+inf) -> change it to (-1)
correctedScan_msg.ranges = [0] * len(data.ranges)
for i in range(len(data.ranges)):
if numpy.isinf(data.ranges[i]):
correctedScan_msg.ranges[i] = -100
else:
correctedScan_msg.ranges[i] = data.ranges[i]
#### Publish msg
rate = rospy.Rate(1800) # 1800hz
correctedScan_publisher.publish(correctedScan_msg)
rospy.loginfo("\n/scanCorrected is now being published...")
rate.sleep()
def _processLaserscan(self, readingType, remappedTimestamp, content):
# FIXME: For type ROBOTLASER, we should publish the robot/sensor pose using TF and use the correct TF frame
laserscan = LaserScan()
laserscan.header = Header(stamp=remappedTimestamp, frame_id="odom", seq=self._laserscanCounter)
if readingType.startswith("RAWLASER") or readingType.startswith("ROBOTLASER"):
laserscan.angle_min = float(content[1])
laserscan.angle_max = laserscan.angle_min + float(content[2])
laserscan.angle_increment = float(content[3])
laserscan.time_increment = 0
laserscan.scan_time = 0.0 # FIXME
laserscan.range_min = 0
laserscan.range_max = float(content[4])
numRanges = int(content[7])
for i in xrange(0, numRanges):
laserscan.ranges.append( float(content[8 + i]) )
numRemissions = int(content[8 + numRanges])
for i in xrange(0, numRemissions):
laserscan.intensities.append( float(content[9 + numRanges + i]) )
else:
rospy.logwarn("Unsupported laser of type %s in line %d" % (readingType, self._lineCounter) )
publisher = self._laserscanPublishers[ self._getLaserID(readingType, content) ]
publisher.publish(laserscan)
self._laserscanCounter += 1
if (self.publishTracks):
self._currentLaserMsgs[self._getLaserID(readingType, content)] = laserscan
def mergescans(lidarscandata):
global camscans, scansmerged
mergedscan = LaserScan()
mergedscan.header.stamp = lidarscandata.header.stamp
mergedscan.header.frame_id = 'laser_frame'
mergedscan.angle_min = 0.0
mergedscan.angle_max = ANGINC * NUMPOINTS
mergedscan.angle_increment = ANGINC
mergedscan.time_increment = 0.0
mergedscan.scan_time = lidarscandata.scan_time
mergedscan.range_min = lidarscandata.range_min
mergedscan.range_max = lidarscandata.range_max
mergedscan.ranges = []
angle = 0.0
while angle <= mergedscan.angle_max:
distance = 0.0
distance = getlidarscan(angle, lidarscandata)
distcam = getcamscan(angle)
if not distcam == 0.0:
distance = distcam
mergedscan.ranges.append(distance)
angle += ANGINC
scan_pub.publish(mergedscan)
camscans = []
def write_hokuyo_4m_to_laserscan(self, utime, data):
"""writes the hokuyo data into a LaserScan message
:param utime: timestamp in microseconds
:param data: list containing the data
:return: timestamp: ros timestamp
scan: LaserScan message
"""
try:
# create a ros timestamp
timestamp = rospy.Time.from_sec(utime / 1e6)
# get hokuyo and base link
hokuyo_urg_link = self.hok_urg_frame
base_link = self.body_frame
# create a LaserScan message
scan = LaserScan()
scan.header.stamp = timestamp
scan.header.frame_id = hokuyo_urg_link
scan.angle_min = -np.pi / 2
scan.angle_max = np.pi / 2
scan.angle_increment = 0.0061359233
scan.time_increment = 1.466e-4
scan.scan_time = 0.1
scan.range_min = 0
scan.range_max = 100.0
scan.ranges = data
# create base_link hokuyo_urg_link static transformer
hok_static_transform_stamped = geometry_msgs.msg.TransformStamped()
hok_static_transform_stamped.header.stamp = timestamp
hok_static_transform_stamped.header.frame_id = base_link
hok_static_transform_stamped.child_frame_id = hokuyo_urg_link
hok_static_transform_stamped.transform.translation.x = 0.31
hok_static_transform_stamped.transform.translation.y = 0
hok_static_transform_stamped.transform.translation.z = 0.38
quat = tf.transformations.quaternion_from_euler(0, 0, 0)
hok_static_transform_stamped.transform.rotation.x = quat[0]
hok_static_transform_stamped.transform.rotation.y = quat[1]
hok_static_transform_stamped.transform.rotation.z = quat[2]
hok_static_transform_stamped.transform.rotation.w = quat[3]
# publish static transform
tf_static_msg = tf2_msgs.msg.TFMessage(
[hok_static_transform_stamped])
return timestamp, scan, tf_static_msg
except Exception as e:
print(e)
def pubSpot(scan, target, targetRange, FRAME_ID):
"""Generate spot scan data from input.
Parameters
----------
scan : LaserScan
scan from LIDAR
target : float or str
target angle in degree
targetRange : float
distance to target in m
FRAME_ID : str
frame id
"""
output = LaserScan()
output.header.seq = scan.header.seq
output.header.stamp.secs = scan.header.stamp.secs
output.header.stamp.nsecs = scan.header.stamp.nsecs
output.header.frame_id = FRAME_ID
output.angle_min = scan.angle_min
output.angle_max = scan.angle_max
output.angle_increment = scan.angle_increment
output.time_increment = scan.time_increment
output.scan_time = scan.scan_time
output.range_min = scan.range_min
output.range_max = scan.range_max
output.ranges = np.zeros(360)
output.intensities = np.zeros(360)
if target != "NaN":
output.ranges[int(target)] = targetRange
output.intensities[int(target)] = 10000.0
pub = rospy.Publisher("spots_laserdata", LaserScan, queue_size=1)
pub.publish(output)
spot = Marker()
spot.id = np.random.random_integers(2**16 - 1)
spot.type = spot.CYLINDER
spot.scale.x = 0.05
spot.scale.y = 0.05
spot.scale.z = 1.0
spot.pose.position.x = targetRange * np.cos(target / 360.0 * 2.0 * np.pi)
spot.pose.position.y = targetRange * np.sin(target / 360.0 * 2.0 * np.pi)
spot.header.frame_id = FRAME_ID
spot.header.stamp = rospy.Time.now()
spot.color.r = 255.0
spot.color.g = 255.0
spot.color.b = 255.0
spot.color.a = 255.0
spot.lifetime.secs = 300
pub = rospy.Publisher(PUB_TO, Marker, queue_size=1)
pub.publish(spot)
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# this approximation works (in radians) for small angles
th = self.th - self.th_pre
self.dr = th / elapsed
# publish the odom information
quaternion = Quaternion()
quaternion.x = self.qx
quaternion.y = self.qy
quaternion.z = self.qz
quaternion.w = self.qw
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(0, 0, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id,
)
self.laserBroadcaster.sendTransform(
(0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), self.laser_frame_id, self.base_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
laser = LaserScan()
laser.header.stamp = now
laser.header.frame_id = self.laser_frame_id
laser.angle_min = self.laser.angle_min
laser.angle_max = self.laser.angle_max
laser.angle_increment = self.laser.angle_increment
laser.time_increment = self.laser.time_increment
laser.scan_time = self.laser.scan_time
laser.range_min = self.laser.range_min
laser.range_max = self.laser.range_max
laser.ranges = self.laser.ranges
laser.intensities = self.laser.intensities
self.laserPub.publish(laser)
def configScannerMsg():
testerPackVal = LaserScan()
testerPackVal.header.frame_id = "laser"
testerPackVal.angle_min = -1.57079637051
testerPackVal.angle_max = 1.56466042995
testerPackVal.angle_increment = 0.00613592332229
testerPackVal.time_increment = 9.76562514552e-05
testerPackVal.scan_time = 0.10000000149
testerPackVal.range_min = 0.019999999553
testerPackVal.range_max = 5.59999990463
return testerPackVal
def create_laser_msg(range_data_array):
ls = LaserScan()
ls.angle_increment = 0.006283185307179586 # 0.36 deg
ls.angle_max = 2.0943951023931953 # 120.0 deg
ls.angle_min = -2.0943951023931953 # -120.0 deg
ls.range_max = 4.0
ls.range_min = 0.02
ls.scan_time = 0.001 # No idea
ls.time_increment = 1.73611115315e-05 # No idea, took from http://comments.gmane.org/gmane.science.robotics.ros.user/5192
ls.header = Header()
ls.header.frame_id = 'laser_link'
ls.ranges = range_data_array
return ls
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 lidar_listener():
port = 5560
kill = False
#init
rospy.loginfo("Initializing LIDAR Listener...");
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(('', port))
laserpub = rospy.Publisher('/scan', LaserScan)
rospy.init_node('lidar_listener')
message = LaserScan();
message.angle_max = 4.1887
message.angle_increment = .0062832
message.scan_time = .1
message.range_min = .02
message.range_max = 4.0
message.header.frame_id = "/base_laser"
#Loop
rospy.loginfo("LIDAR Listener initialized")
while not rospy.is_shutdown():
try:
raw_data =sock.recv(4096, socket.MSG_DONTWAIT)
message.header.stamp=rospy.Time.now()
if not raw_data:
rospy.logwarn("No Raw Data")
else:
message.ranges=decode(raw_data)
print message.ranges
rospy.logdebug(message.ranges)
laserpub.publish(message)
except socket.error as ex:
if (ex[0] != 11):
rospy.logwarn( "Lidar Listener Socket Exception: ",ex)
else:
rospy.logdebug("Lidar Listener Timed Out");
except Exception as ex:
raise ex
rospy.loginfo("Shutting down LIDAR listener...")
sock.close()
def imu_serial():
pub = rospy.Publisher('laser_scan', LaserScan)
rospy.init_node('imu_serial')
laser_msg = LaserScan()
laser_msg.header.frame_id = 'laser'
laser_msg.angle_min = -1.5
laser_msg.angle_max = 1.5
laser_msg.angle_increment = 0.1
laser_msg.time_increment = 0.1
laser_msg.scan_time = 0.1
laser_msg.range_min = 0.5
laser_msg.range_max = 1.5
laser_msg.ranges = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 9.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.1, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 1.0]
laser_msg.intensities = laser_msg.ranges
r = rospy.Rate(100) # 10hz
while not rospy.is_shutdown():
laser_msg.header.stamp = rospy.get_rostime()
pub.publish(laser_msg)
r.sleep()
def build_constant_scan(
range_val, intensity_val,
angle_min, angle_max, angle_increment, scan_time):
count = np.uint(np.ceil((angle_max - angle_min) / angle_increment))
if count < 0:
raise BuildScanException
scan = LaserScan()
scan.header.stamp = rospy.rostime.Time.from_sec(10.10)
scan.header.frame_id = "laser_frame"
scan.angle_min = angle_min
scan.angle_max = angle_max
scan.angle_increment = angle_increment
scan.scan_time = scan_time.to_sec()
scan.range_min = PROJECTION_TEST_RANGE_MIN
scan.range_max = PROJECTION_TEST_RANGE_MAX
scan.ranges = [range_val for _ in range(count)]
scan.intensities = [intensity_val for _ in range(count)]
scan.time_increment = scan_time.to_sec()/count
return scan
def default(self, ci='unused'):
laserscan = LaserScan()
laserscan.header = self.get_ros_header()
# Note: Scan time and laser frequency are chosen as standard values
laser_frequency = 40 # TODO ? component_instance.frequency()
scan_window = self.component_instance.bge_object['scan_window']
num_readings = scan_window / self.component_instance.bge_object['resolution']
laserscan.angle_max = scan_window * math.pi / 360
laserscan.angle_min = laserscan.angle_max * -1
laserscan.angle_increment = scan_window / num_readings * math.pi / 180
laserscan.time_increment = 1 / laser_frequency / num_readings
laserscan.scan_time = 1.0
laserscan.range_min = 0.3
laserscan.range_max = self.component_instance.bge_object['laser_range']
# ROS expect the ranges to be sorted clockwise.
# see morse.builder.sensor.LaserSensorWithArc.create_laser_arc
# where we create the ray from -window / 2.0 to +window / 2.0
laserscan.ranges = self.data['range_list']
self.publish(laserscan)
def _msg(self, ranges, intensities, scan_time):
new_time = Time.now()
delta_time = new_time - self._last_time
self._last_time = new_time
msg = LaserScan()
msg.header.frame_id = self._frame_id
msg.header.stamp = Time.now()
msg.angle_max = self.__MAX_ANGLE
msg.angle_min = self.__MIN_ANGLE
msg.angle_increment = self.__ANGLE_INCREMENT
# Note: time_increment is the time between measurements, i.e. how often we read the scan and publish it (in
# seconds)
msg.time_increment = 0.1 #delta_time.secs
# Note: scan_time is the time between scans of the laser, i.e., the time it takes to read 360 degrees.
msg.scan_time = 0.1 # scan_time
msg.range_min = float(self._min_range)
msg.range_max = float(self._max_range)
msg.ranges = [min(max(range, msg.range_min), msg.range_max) for range in ranges]
msg.intensities = intensities
return msg
def create_lidar_msg(self, L):
raw_lidar = L.data
#stripped_lidar = raw_lidar.translate(None, '[]').translate(None, '"').translate(None, '\'')
array_lidar = raw_lidar.split(";")
lidar_msg = LaserScan()
lidar_msg.header = self.create_header() #self?
lidar_msg.angle_min = math.radians(float(array_lidar[0]))
lidar_msg.angle_max = math.radians(float(array_lidar[1]))
lidar_msg.angle_increment = math.radians(0.25) #MAKE PARAM
lidar_msg.time_increment = 0.025/(270*4) #time in ms / measurements YOYOYOYO CHECK THIS
lidar_msg.scan_time = float(array_lidar[2]) / 1000 #time in ms
lidar_msg.range_min = float(array_lidar[4]) / 1000 #sent in mm, should be meters
lidar_msg.range_max = float(array_lidar[5]) / 1000 #sent in mm, should be meters
array_string = array_lidar[3] #.translate(None, '[]')
string_array = array_string.split(",")
lidar_msg.ranges = [float(r) / 1000 for r in string_array] #better way?
self.scanPub.publish(lidar_msg)
def create_lidar_msg(L):
raw_lidar = L.data
stripped_lidar = raw_lidar.translate(None, '[]').translate(None, '"').translate(None, '\'')
array_lidar = stripped_lidar.split(",")
num_readings = 1440
scan = LaserScan()
scan.header.stamp = rospy.Time.now()
scan.header.frame_id = "base_scan"
scan.angle_min = math.radians(float(array_lidar[0]))
scan.angle_max = math.radians(float(array_lidar[1]))
scan.angle_increment = math.radians(.25) #get from lidar
scan.time_increment = float(25. / num_readings) #time in ms / measurements
scan.scan_time = float(array_lidar[2])
scan.range_min = float(array_lidar[4]) / 1000 #sent in mm, needs m
scan.range_max = float(array_lidar[5]) / 1000 #sent in mm, needs m
# string_array = array_lidar[3].strip("[").strip("]").split(",")
array_string = array_lidar[3].translate(None, '[]')
string_array = array_string.split(",")
scan.ranges = [float(r) / 1000 for r in string_array] #better way?
scanPub.publish(scan)
def sonar():
global port
rospy.init_node('Sonar_Array')
rospy.loginfo( "Sonar Array serial connection is running on " + port)
pub = rospy.Publisher('sonar_data',LaserScan)
sequence = 0
while not rospy.is_shutdown():
try:
with serial.Serial(port = port, baudrate = 115200) as ser:
while not rospy.is_shutdown():
if VERBOSE:
print "Starting..."
ser.flushInput()
line = ser.readline()
if VERBOSE:
print "Got line! ", line
tokens = line.split()
if VERBOSE:
print "Number of tokens: ", len(tokens)
if len(tokens) is 13:
if VERBOSE:
print '-'+tokens[0]+'-'
if(tokens[0] == 'Sonars:'):
p = LaserScan()
p.header.stamp = rospy.get_rostime()
p.header.frame_id = 'sonar_array_frame'
p.header.seq = sequence
p.angle_min = -pi/2
p.angle_max = pi/2
p.angle_increment = pi/11
p.scan_time = .1
p.ranges = [Process(i) for i in tokens[1:13]]
p.ranges.reverse()
pub.publish(p)
except IOError:
print "Disconnected? (Error)"
time.sleep(1)
if tokens[0] == 'FLASER':
msg = LaserScan()
num_scans = int(tokens[1])
if num_scans != 180 or len(tokens) < num_scans + 9:
rospy.logwarn("unsupported scan format")
continue
msg.header.frame_id = 'laser'
t = rospy.Time(float(tokens[(num_scans + 8)]))
msg.header.stamp = t
msg.angle_min = -90.0 / 180.0 * pi
msg.angle_max = 90.0 / 180.0 * pi
msg.angle_increment = pi / (num_scans - 1.0)
msg.time_increment = 0.2 / 360.0
msg.scan_time = 0.2
msg.range_min = 0.001
msg.range_max = 50.0
msg.ranges = [float(r) for r in tokens[2:(num_scans + 2)]]
bag.write('scan', msg, t)
#odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 2):(num_scans + 5)]]
#tf_msg = make_tf_msg(odom_x, odom_y, odom_theta, t)
#bag.write('tf', tf_msg, t)
#odom_msg = mak_odom_msg(odom_x, odom_y, odom_theta,t)
#bag.write("odom", odom_msg, t)
elif tokens[0] == 'ODOM':
odom_x, odom_y, odom_theta = [float(t) for t in tokens[1:4]]
t = rospy.Time(float(tokens[7]))
import rospy
from cwru_base.msg import Sonar
from sensor_msgs.msg import LaserScan
scan=LaserScan()
def handle_sonar(msg, scan_pub):
scan.header.frame_id = msg.header.frame_id
scan.header.stamp = msg.header.stamp
scan.ranges = [msg.dist for i in range(0,30)]
scan_pub.publish(scan)
if __name__ == '__main__':
rospy.init_node('sonar_translator')
scan_pub = rospy.Publisher('sonar_scan', LaserScan)
rospy.Subscriber('sonar', Sonar, handle_sonar, scan_pub)
scan.angle_min = rospy.get_param("~angle_min", -0.19)
scan.angle_max = rospy.get_param("~angle_max", 0.19)
scan.angle_increment = rospy.get_param("~angle_increment", 0.01266666667)
scan.time_increment = rospy.get_param("~time_increment", 0.0)
scan.scan_time = rospy.get_param("~scan_time", 0.05)
scan.range_min = rospy.get_param("~range_min", 0.10)
scan.range_max = rospy.get_param("~range_max", 2.0)
rospy.spin()
#!/usr/bin/python
import rospy
import sys
from sensor_msgs.msg import LaserScan
rospy.init_node('laserz')
pub = rospy.Publisher('/base_scan', LaserScan)
scan = LaserScan()
scan.header.frame_id = '/base_laser_link'
scan.angle_min = -2.26892805099
scan.angle_max = 2.26456475258
scan.angle_increment = 0.00436332309619
scan.time_increment = 1.73611115315e-05
scan.scan_time = 0.0500000007451
scan.range_min = 0.0230000000447
scan.range_max = 60.0
rate = rospy.Rate(20)
while not rospy.is_shutdown():
rate.sleep()
scan.header.stamp = rospy.Time.now()
if '-x' in sys.argv:
scan.ranges = [1.0]*200
if int(scan.header.stamp.secs) % 10 < 5:
scan.ranges += [float('inf')] * 640
else:
scan.ranges += [1.5] * 640
scan.ranges += [1.0]*200
else:
scan.ranges= [float('inf')] * 1040
def _BroadcastOdometryInfo(self, lineParts):
# This broadcasts ALL info from the Propeller based robot every time data comes in
partsCount = len(lineParts)
#rospy.logwarn(partsCount)
if (partsCount < 8): # Just discard short lines, increment this as lines get longer
pass
try:
x = float(lineParts[1])
y = float(lineParts[2])
# 3 is odom based heading and 4 is gyro based
# If there is some way to "integrate" these, go for it!
theta = float(lineParts[4])
vx = float(lineParts[5])
omega = float(lineParts[6])
#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_link over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
#This transform conflicts with transforms built into the Turtle stack
# http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29
# This is done in/with the robot_pose_ekf because it can integrate IMU/gyro data
# using an "extended Kalman filter"
# REMOVE this "line" if you use robot_pose_ekf
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rosNow,
"base_footprint",
"odom"
)
# next, we'll publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = rosNow
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
#for Turtlebot stack from turtlebot_node.py
# robot_pose_ekf needs these covariances and we may need to adjust them?
# From: ~/turtlebot/src/turtlebot_create/create_node/src/create_node/covariances.py
# This is not needed if not using robot_pose_ekf
'''
odometry.pose.covariance = [1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3]
odometry.twist.covariance = [1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3]
'''
self._OdometryPublisher.publish(odometry)
#"IMU" data from Gyro
'''
# Based on code in TurtleBot source:
# ~/turtlebot/src/turtlebot_create/create_node/src/create_node/gyro.py
# It may make more sense to compute some of this on the Propeller board,
# but for now I'm just trying to follow the TurtleBot Create code as beast I can
current_time = rosNow
#dt = (current_time - last_time).to_sec()
#past_orientation = self.orientation
#self.imu_data.header.stamp = sensor_state.header.stamp
#self.imu_data.angular_velocity.z = (float(sensor_state.user_analog_input)-self.cal_offset)/self.cal_offset*self.gyro_measurement_range*(math.pi/180.0)*self.gyro_scale_correction
#sign change
#self.imu_data.angular_velocity.z = -1.0*self.imu_data.angular_velocity.z
#self.orientation += self.imu_data.angular_velocity.z * dt
#print orientation
#(self.imu_data.orientation.x, self.imu_data.orientation.y, self.imu_data.orientation.z, self.imu_data.orientation.w) = PyKDL.Rotation.RotZ(self.orientation).GetQuaternion()
#self.imu_data = odom.twist.twist = Twist(Vector3(d/dt, 0, 0), Vector3(0, 0, angle/dt))
#self.imu_pub.publish(self.imu_data)
#self.imu_data.header.stamp = sensor_state.header.stamp
#self.imu_data.angular_velocity.z = (float(sensor_state.user_analog_input)/self.gyro_measurement_range*(math.pi/180.0)*self.gyro_scale_correction)
#sign change
#self.imu_data.angular_velocity.z = -1.0*self.imu_data.angular_velocity.z
#raw_orientation = past_orientation + self.imu_data.angular_velocity.z * dt
#print orientation
#(self.imu_data.orientation.x, self.imu_data.orientation.y, self.imu_data.orientation.z, self.imu_data.orientation.w) = PyKDL.Rotation.RotZ(raw_orientation).GetQuaternion()
#self.imu_pub_raw.publish(self.imu_data)
last_time = current_time
imu = Imu()
imu.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] # From Turtlebot, probably wrong.
# You CANNOT set the orientation_covariance to -1, 0, ..., else you get this error:
#[ERROR] [1405831732.096853617]: Covariance specified for measurement on topic imu is zero
# The TurtleBot Create builds this in Python, but I'm not sure if I can or want to build orientation
# myself? Does the Gyro give this?
#imu.orientation_covariance = [-1,0,0,0,0,0,0,0,0] # This should indicate no data for this matrix
imu.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] # From Turtlebot, probably wrong.
#imu.angular_velocity_covariance = [-1,0,0,0,0,0,0,0,0] # This should indicate no data for this matrix
imu.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0] # This should indicate no data for this matrix
#imu.orientation_covariance = [999999 , 0 , 0, 0, 9999999, 0, 0, 0, 999999]
#imu.angular_velocity_covariance = [9999, 0 , 0, 0 , 99999, 0, 0 , 0 , 0.02]
#imu.linear_acceleration_covariance = [0.2 , 0 , 0, 0 , 0.2, 0, 0 , 0 , 0.2]
imu.linear_acceleration.x = 0
imu.linear_acceleration.y = 0
imu.linear_acceleration.z = 0
imu.angular_velocity.x = float(lineParts[7]) / 57.2957795130824
imu.angular_velocity.y = float(lineParts[8]) / 57.2957795130824
imu.angular_velocity.z = float(lineParts[9]) / 57.2957795130824
imu.orientation.x = 0
imu.orientation.y = 0
imu.orientation.z = 0
imu.orientation.w = 1.0
#imu.header.stamp = rospy.Time.now()
imu.header.stamp = rosNow
imu.header.frame_id = "gyro_link"
#imu.header.frame_id = 'base_link'
self._ImuPublisher.publish(imu)
'''
# Joint State for Turtlebot stack
# Note without this transform publisher the wheels will
# be white, stuck at 0, 0, 0 and RVIZ will tell you that
# there is no transform from the wheel_links to the base_
'''
# Instead of publishing a stream of pointless transforms,
# How about if I just make the joint static in the URDF?
# create.urdf.xacro:
# <joint name="right_wheel_joint" type="fixed">
# NOTE This may prevent Gazebo from working with this model
js = JointState(name = ["left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint"],
position=[0,0,0,0], velocity=[0,0,0,0], effort=[0,0,0,0])
js.header.stamp = rosNow
self.joint_states_pub.publish(js)
'''
# Fake laser from "PING" Ultrasonic Sensor input:
# http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF
# Transform: http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29
'''
# We don't need to broadcast a transform,
as it is static and contained within the URDF files
self._SonarTransformBroadcaster.sendTransform(
(0.1, 0.0, 0.2),
(0, 0, 0, 1),
rosNow,
"sonar_laser",
"base_link"
)
'''
# Some help: http://books.google.com/books?id=2ZL9AAAAQBAJ&pg=PT396&lpg=PT396&dq=fake+LaserScan+message&source=bl&ots=VJMfSYXApG&sig=s2YgiHTA3i1OjVyPxp2aAslkW_Y&hl=en&sa=X&ei=B_vDU-LkIoef8AHsooHICA&ved=0CG0Q6AEwCQ#v=onepage&q=fake%20LaserScan%20message&f=false
num_readings = 360 # How about 1 per degree?
laser_frequency = 100 # I'm not sure how to decide what to use here.
#ranges = [1] * num_readings # Fill array with fake "1" readings for testing
ranges = [0] * num_readings # Fill array with 0 and then overlap with real readings
pingRange0 = int(lineParts[7]) / 100.0
ranges[0] = pingRange0
ranges[1] = pingRange0
ranges[2] = pingRange0
ranges[3] = pingRange0
ranges[4] = pingRange0
ranges[5] = pingRange0
ranges[359] = pingRange0
ranges[358] = pingRange0
ranges[357] = pingRange0
ranges[356] = pingRange0
ranges[355] = pingRange0
# Is there a more concise way to code that array fill?
# LaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html
sonar_scan = LaserScan()
sonar_scan.header.stamp = rosNow
sonar_scan.header.frame_id = "ping_sensor_array"
# For example:
#scan.angle_min = -45 * M_PI / 180; // -45 degree
#scan.angle_max = 45 * M_PI / 180; // 45 degree
# if you want to receive a full 360 degrees scan, you should try setting min_angle to -pi/2 and max_angle to 3/2 * pi.
# Radians: http://en.wikipedia.org/wiki/Radian#Advantages_of_measuring_in_radians
sonar_scan.angle_min = 0
sonar_scan.angle_max = 2 * 3.14159 # Full circle
sonar_scan.scan_time = 1 # I think this is only really applied for 3D scanning
# Make sure the part you divide by num_readings is the same as your angle_max!
# Might even make sense to use a variable here?
sonar_scan.angle_increment = (2 * 3.14) / num_readings
sonar_scan.time_increment = (1 / laser_frequency) / (num_readings)
# From: http://www.parallax.com/product/28015
# Range: approximately 1 inch to 10 feet (2 cm to 3 m)
# This should be adjusted based on the imaginary distance between the actual laser
# and the laser location in the URDF file. Or else the adjustment somewhere else?
sonar_scan.range_min = 0.02 # in Meters Distances below this number will be ignored
sonar_scan.range_max = 3 # in Meters Distances above this will be ignored
sonar_scan.ranges = ranges
# "intensity" is a value specific to each laser scanner model.
# It can safely be ignored
self._SonarPublisher.publish(sonar_scan)
except:
rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
def spin(self):
encoders = [0,0]
self.x = 0 # position in xy plane
self.y = 0
self.th = 0
# NEED to reorder the laser scans and flip the laser around... this will not be intuitive for students!!
# things that don't ever change
scan_link = rospy.get_param('~frame_id',ROBOT_NAME+'base_laser_link')
scan = LaserScan(header=rospy.Header(frame_id=ROBOT_NAME+scan_link))
scan.angle_min = -pi
scan.angle_max = pi
scan.angle_increment = pi/180.0
scan.range_min = 0.020
scan.range_max = 5.0
scan.time_increment = 1.0/(5*360)
scan.scan_time = 0.2
odom = Odometry(header=rospy.Header(frame_id=ROBOT_NAME+"odom"), child_frame_id=ROBOT_NAME+'base_link')
# main loop of driver
r = rospy.Rate(5)
iter_count = 0
rospy.sleep(4)
#self.robot.requestScan()
scan.header.stamp = rospy.Time.now()
last_motor_time = rospy.Time.now()
total_dth = 0.0
while not rospy.is_shutdown():
#print "spinning"
iter_count += 1
if self.cmd_to_send != None:
self.robot.send_command(self.cmd_to_send)
self.cmd_to_send = None
t_start = time.time()
self.robot.requestScan()
#time.sleep(.01)
delta_t = (rospy.Time.now() - scan.header.stamp).to_sec()
if delta_t-0.2 > 0.1:
print "Iteration took longer than expected (should be 0.2) ", delta_t
scan.header.stamp = rospy.Time.now()
(scan.ranges, scan.intensities) = self.robot.getScanRanges()
# repeat last measurement to simulate -pi to pi (instead of -pi to pi - pi/180)
scan.ranges.append(scan.ranges[0])
scan.intensities.append(scan.intensities[0])
#print 'Got scan ranges %f' % (time.time() - t_start)
# get motor encoder values
curr_motor_time = rospy.Time.now()
t_start = time.time()
try:
start_t = rospy.Time.now()
left, right, left_speed, right_speed = self.robot.getMotors()
delta_t = (rospy.Time.now() - scan.header.stamp).to_sec()
# now update position information
# might consider moving curr_motor_time down
dt = (curr_motor_time - last_motor_time).to_sec()
last_motor_time = curr_motor_time
d_left = (left - encoders[0])/1000.0
d_right = (right - encoders[1])/1000.0
# might consider using speed instead!
#print d_left, d_right, left_speed*dt/1000.0, right_speed*dt/1000.0
#d_left, d_right = left_speed*dt/1000.0, right_speed*dt/1000.0
encoders = [left, right]
dx = (d_left+d_right)/2
dth = (d_right-d_left)/(BASE_WIDTH/1000.0)
total_dth += dth
x = cos(dth)*dx
y = -sin(dth)*dx
self.x += cos(self.th)*x - sin(self.th)*y
self.y += sin(self.th)*x + cos(self.th)*y
self.th += dth
# prepare tf from base_link to odom
quaternion = Quaternion()
quaternion.z = sin(self.th/2.0)
quaternion.w = cos(self.th/2.0)
# prepare odometry
odom.header.stamp = curr_motor_time
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx/dt
odom.twist.twist.angular.z = dth/dt
self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), curr_motor_time, ROBOT_NAME+"base_link", ROBOT_NAME+"odom" )
self.odomPub.publish(odom)
#print 'Got motors %f' % (time.time() - t_start)
except Exception as err:
print "my error is " + str(err)
t_start = time.time()
self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
try:
bump_sensors = self.robot.getDigitalSensors()
self.bumpPub.publish(Bump(leftFront=bump_sensors[0],leftSide=bump_sensors[1],rightFront=bump_sensors[2],rightSide=bump_sensors[3]))
except:
print "failed to get bump sensors!"
# # send updated movement commands
#print 'Set motors %f' % (time.time() - t_start)
# publish everything
#print "publishing scan!"
self.scanPub.publish(scan)
# wait, then do it again
r.sleep()
odomPub = rospy.Publisher('roombaOdom',Odometry)
odoMsg = Odometry()
odoMsg.header.frame_id = '/odom'
odoMsg.child_frame_id = '/odom'
odoMsg.pose.covariance = [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0]
#/scan publisher
scanPub = rospy.Publisher('scan',LaserScan)
scanMsg = LaserScan()
scanMsg.header.frame_id = 'ir_base'
scanMsg.angle_min = 0.01
scanMsg.angle_max = 0.01
scanMsg.angle_increment = 0.02
scanMsg.time_increment = 0.0
scanMsg.scan_time = 0.0
scanMsg.range_min = 0.02
scanMsg.range_max = 1.5
scanMsg.intensities = []
#robot joint state (joint angles in radians)
left_wheel_angle = 0.0;
right_wheel_angle = 0.0;
# Open a file
logFile = open("log.txt", "r")
# robot states w.r.t inertial frame
robot_x_i = 0.0
