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 _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 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 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 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 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 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 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 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 main():
rospy.init_node('test') # to initiate and create an unique node
rospy.Subscriber(
'/tim561_scan', LaserScan,
callback) # subscribing to the topic from lidar_test_data.bag
pub = rospy.Publisher('/tim561_scan_filtered', LaserScan,
queue_size=10) # Publisher for filtered data
while laserscan is None and not rospy.is_shutdown(
): # For when data is not available keeping the node on hold
rospy.sleep(1)
rate = rospy.Rate(10)
new_data = LaserScan() # Object for filtered data
while not rospy.is_shutdown():
new_data.header = laserscan.header # Header is not changed in new data
new_data.header.frame_id = 'laser' # ID changed for rviz visualization
new_data.angle_min = laserscan.angle_min # Doesn't change after filtering
new_data.angle_max = laserscan.angle_max # Doesn't change after filtering
new_data.angle_increment = laserscan.angle_increment # Doesn't change after filtering
new_data.time_increment = laserscan.time_increment # Doesn't change after filtering
new_data.ranges = lowpass(laserscan.ranges, laserscan.angle_increment,
RC) # Filtering the data
new_data.range_min = min(
new_data.ranges) # Finding new minimum of filtered data
new_data.range_max = max(
new_data.ranges) # Finding new maximum of filtered data
new_data.intensities = laserscan.intensities # Doesn't change after filtering(empty)
pub.publish(new_data) # Publishing filtered data
rate.sleep()
def callback(msg, ir_sensors):
# Receive raw data and publish a value
# The raw_data variable should be an array of numbers
raw_data = msg.ranges
distances_avg = ir_sensors.rawDataToDistance(raw_data)
front_distances = distances_avg[0:6]
obstacle_distances = distances_avg[6:]
theta_p, distance, num_sensors, rotation_direction, pose = ir_sensors.distanceToPose(
front_distances, msg.header
) # only take first 6 distances, those rorrespond to the front sensors
rospy.loginfo(
'Angle: {:.0f}, Distance: {:.2f}, Num Sensors: {:d}, Rot Dir: {:d}'.
format(theta_p, distance, num_sensors, rotation_direction))
rospy.loginfo(pose)
pub_human_angle.publish(Float64(theta_p))
pub_distance.publish(Float64(distance))
pub_num_sensors.publish(Int8(num_sensors))
pub_rotation_dir.publish(Int8(rotation_direction))
pub_ir_pose.publish(pose)
other_sensors = LaserScan()
other_sensors.header = msg.header
other_sensors.ranges = obstacle_distances
pub_ir_other_sensors_distance.publish(other_sensors)
obstacles = [0, 0, 0, 0, 0, 0, 0]
for i in range(len(obstacle_distances)):
if obstacle_distances[i] < ir_sensors.obstacle_distance_min:
obstacles[i] = 1
rospy.set_param_raw('obstacles', obstacles)
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 _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 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 laser_callback(self, msg):
self.laser = msg
self.get_transform()
#rospy.loginfo(msg)
transformed_laser = LaserScan()
transformed_laser.header = msg.header
self.laser_pub.publish(msg)
def publish_scan(self, angles, ranges):
ls = LaserScan()
ls.header = Utils.make_header("laser", stamp=self.last_stamp)
ls.angle_min = np.min(angles)
ls.angle_max = np.max(angles)
ls.angle_increment = np.abs(angles[0] - angles[1])
ls.range_min = 0
ls.range_max = np.max(ranges)
ls.ranges = ranges
self.pub_fake_scan.publish(ls)
def publish_scan(self, angles, ranges):
ls = LaserScan()
ls.header = Utils.make_header("inferred_scan_frame") # This fills in current time for us.
ls.angle_min = np.min(angles)
ls.angle_max = np.max(angles)
ls.angle_increment = np.abs(angles[0] - angles[1])
ls.range_min = 0
ls.range_max = np.max(ranges)
ls.ranges = ranges
self.pub_fake_scan.publish(ls)
def publish_scan(self, angles, ranges):
ls = LaserScan()
ls.header = Utils.make_header("laser", stamp=self.last_stamp)
ls.angle_min = np.min(angles)
ls.angle_max = np.max(angles)
ls.angle_increment = np.abs(angles[0] - angles[1])
ls.range_min = 0
ls.range_max = np.max(ranges)
ls.ranges = ranges
self.pub_fake_scan.publish(ls)
def publish_scan(self, angles, ranges):
'''publish fake laserscan generated from the mean expected pose of particles distribution'''
ls = LaserScan()
ls.header = self.create_header("base_laser_link")
ls.angle_min = np.min(angles)
ls.angle_max = np.max(angles)
ls.angle_increment = np.abs(angles[0] - angles[1])
ls.range_min = 0
ls.range_max = np.max(ranges)
ls.ranges = ranges
self.pub_fake_scan.publish(ls)
def publish_scan(self, angles, ranges):
# publish the given angels and ranges as a laser scan message
ls = LaserScan()
ls.header = make_header("laser", stamp=self.last_stamp) # Utils
ls.angle_min = np.min(angles)
ls.angle_max = np.max(angles)
ls.angle_increment = np.abs(angles[0] - angles[1])
ls.range_min = 0
ls.range_max = np.max(ranges)
ls.ranges = ranges
self.pub_fake_scan.publish(ls)
def laserCallback(self, msg):
pos = self.controllerPosition
# angle = -math.atan2(pos.point.y, pos.point.x)
# distance = math.sqrt((pos.point.x) ** 2 + (pos.point.y) ** 2 + (pos.point.z) ** 2)
# BUFFER = 2 # arc length in meters
# arc = BUFFER / distance # in radians
# minAngle = max(angle - (arc / 2), msg.angle_min)
# minAngleIndex = int(minAngle / msg.angle_increment + abs(msg.angle_min / msg.angle_increment))
# maxAngle = min(angle + (arc / 2), msg.angle_max)
# maxAngleIndex = int(maxAngle / msg.angle_increment + abs(msg.angle_min / msg.angle_increment))
laserPoints = list(msg.ranges) # [minAngleIndex:maxAngleIndex]
collisionScan = LaserScan()
collisionScan.header = msg.header
collisionScan.angle_min = msg.angle_min #minAngle
collisionScan.angle_max = msg.angle_max #maxAngle
collisionScan.angle_increment = msg.angle_increment
collisionScan.time_increment = msg.time_increment
collisionScan.scan_time = msg.scan_time
collisionScan.range_min = msg.range_min
collisionScan.range_max = msg.range_max
minDist = 999999
minDistAngle = 0
for i, point in enumerate(laserPoints):
#pointAngle = (i + minAngleIndex) * msg.angle_increment + msg.angle_min
pointAngle = i * msg.angle_increment + msg.angle_min
if self.controllerPosition is not None:
distanceFromController = math.sqrt((math.cos(pointAngle) * point - pos.x) ** 2 + (math.sin(pointAngle) * point - pos.y) ** 2)
if distanceFromController < 1:
laserPoints[i] = 5
if point > msg.range_min and point < minDist:
minDist = point
minDistAngle = pointAngle
if minDist < 0.6:
rospy.loginfo("Minimum distance is {} at {} degrees".format(round(minDist, 3), round(math.degrees(minDistAngle))))
if minDist < 0.3:
self.safeToTrack = False
cmd = Twist()
cmd.linear.x = -0.3
self.cmdvel.publish(cmd)
if self.controllerID is not None or self.controllerPosition is not None:
rospy.loginfo("Temporarily killed controller due to obstacle")
# For permanent killing of controller
# self.controllerID = None
# self.controllerPosition = None
else:
self.safeToTrack = True
collisionScan.ranges = laserPoints
self.laser_pub.publish(collisionScan)
def publish_scan(scan_pub):
scan_msg = LaserScan()
scan_msg.header = Header()
scan_msg.header.frame_id = "world"
scan_msg.header.stamp = rospy.Time.now()
scan_msg.angle_min = -3.1400001049
scan_msg.angle_max = 3.1400001049
scan_msg.angle_increment = 0.0174930356443
scan_msg.range_min = 0.20000000298
scan_msg.range_max = 60.0
scan_msg.ranges = np.random.rand(360)*(60.0 - 0.2) + 0.2
scan_pub.publish(scan_msg)
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 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 reading(self, msg):
resp = LaserScan()
resp.header = msg.header
resp.angle_min = msg.angle_min
resp.angle_max = msg.angle_max
resp.angle_increment = msg.angle_increment
resp.time_increment = msg.time_increment
resp.scan_time = msg.scan_time
resp.range_min = msg.range_min
resp.range_max = msg.range_max
resp.ranges = [self.fix(x, msg.range_max) for x in msg.ranges]
resp.intensities = msg.intensities
self.pub.publish(resp)
def __publish_scan(self):
scan = LaserScan()
scan.header = self.header
scan.header.frame_id = self.tf_id
scan.angle_min = self.roi_angle_min
scan.angle_max = self.roi_angle_max
scan.angle_increment = self.angle_increment
scan.time_increment = self.time_increment
scan.range_min = self.range_min
scan.range_max = self.range_max
scan.ranges = self.ranges
scan.intensities = self.roi_intensities
self.pub.publish(scan)
def publish_laser_scan(self, ranges, depth_msg):
new_msg = LaserScan()
# TODO: Add timing information
new_msg.header = depth_msg.header
new_msg.angle_min = -self._to_rad(self.fov / 2)
new_msg.angle_max = self._to_rad(self.fov / 2)
new_msg.angle_increment = self._to_rad(self.fov) / self.NUM_BUCKETS
new_msg.time_increment = 0.0
new_msg.range_min = 0.0
new_msg.range_max = float('inf')
new_msg.ranges = ranges
new_msg.intensities = [0.0] * len(ranges)
#print(new_msg)
self.laser_pub.publish(new_msg)
def ranges_to_laserscan(self):
# TODO: optimize if bottleneck
msg = LaserScan()
msg.header = Utils.make_header('laser')
msg.angle_min = self.laser_params["angle_min"]
msg.angle_max = self.laser_params["angle_max"]
msg.angle_increment = self.laser_params["angle_increment"]
msg.time_increment = self.laser_params["time_increment"]
msg.scan_time = self.laser_params["scan_time"]
msg.range_min = self.laser_params["range_min"]
msg.range_max = self.laser_params["range_max"]
msg.ranges = self.ranges.tolist()
msg.intensities = self.ranges.tolist()
return msg
def initRangeFinder(self, frame, angle_min, angle_max, range_min,
range_max, ranges_dim):
result = LaserScan()
result.header = Header()
result.header.frame_id = frame
result.header.stamp = self.get_clock().now().to_msg()
result.angle_min = angle_min
result.angle_max = angle_max
result.angle_increment = (angle_max - angle_min) / ranges_dim
result.range_min = range_min
result.range_max = range_max
result.ranges = [0.0] * ranges_dim
return result
def pub_laser(self, msg):
laser = LaserScan()
laser.header = msg.header
laser.header.frame_id = self.frame_id
laser.angle_min = 0
laser.angle_max = 3.14
laser.angle_increment = 0.01
laser.time_increment = 0.01
laser.scan_time = 0.1
laser.range_min = 0.2
laser.range_max = 4.5
laser.ranges = [msg.range, msg.range]
laser.intensities = [1, 1]
self.laserPub.publish(laser)
def filterScanData(self, data):
self.lidarScan = data
newRanges = self.filterRanges()
msg = LaserScan()
msg.header = data.header
msg.angle_min = data.angle_min
msg.angle_max = data.angle_max
msg.angle_increment = data.angle_increment
msg.time_increment = data.time_increment
msg.scan_time = data.scan_time
msg.range_min = data.range_min
msg.range_max = data.range_max
msg.ranges = newRanges
msg.intensities = data.intensities
self.filterScanPub.publish(msg)
def convert_array_to_laser_scan(self, vision_raw_scan):
if vision_raw_scan.size < 100:
return None
header = Header()
header.frame_id = "vision_scan"
#header.stamp = time()
laser_scan = LaserScan()
laser_scan.angle_min = 0.0
laser_scan.angle_max = self.angle_max
laser_scan.angle_increment = self.angle_increment
laser_scan.range_min = 0.0
laser_scan.range_max = self.range_max
#laser_scan.ranges = [0]*360
image_size = vision_raw_scan.shape
if len(image_size) == 3:
vision_raw_scan = cv2.cvtColor(vision_raw_scan, cv2.COLOR_BGR2GRAY)
image_size = vision_raw_scan.shape
if self.init is False:
self._init_lines(image_size)
self.init = True
tasks = list()
for line in range(self.number_lines):
tasks.append((vision_raw_scan, self.lines[line]))
laser_scan.ranges = self.pool.map(_getObstacle, tasks)
#pool.close()
laser_scan.header = header
#laser_scan.scan_time = 1.0/5.0
#laser_scan.time_increment = 1.0/5.0
return laser_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 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)