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)