def callback(self, sensor): #rospy.loginfo(rospy.get_caller_id() + ' I heard %s',sensor.name) laserscan = LaserScan() laserscan.header.stamp = rospy.Time.now() laserscan.header.seq = self.sequence laserscan.header.frame_id = self.default_frame_id self.sequence = self.sequence + 1 if (sensor.name == "Left_Side_Bumper"): laserscan.angle_min = 0.6 laserscan.angle_max = 1 laserscan.range_min = 0.0 laserscan.range_max = 0.3 laserscan.ranges = [0.29, 0.29] elif (sensor.name == "Left_Bumper"): laserscan.angle_min = 0.6 laserscan.angle_max = 1 laserscan.range_min = 0.0 laserscan.range_max = 0.3 laserscan.ranges = [0.29, 0.29] elif (sensor.name == "Right_Bumper"): laserscan.angle_min = -0.6 laserscan.angle_max = 0 laserscan.range_min = 0.0 laserscan.range_max = 0.3 laserscan.ranges = [0.29, 0.29] elif (sensor.name == "Right_Side_Bumper"): laserscan.angle_min = -0.6 laserscan.angle_max = 0 laserscan.range_min = 0.0 laserscan.range_max = 0.3 laserscan.ranges = [0.29, 0.29] #rospy.loginfo(laserscan) self.pub.publish(laserscan)
def ls_scan_pub(self, pub, laser_distance1, laser_angle1): # distance data is from -pi to pi laser_distance1 = np.array(laser_distance1) scan_msg = LaserScan() point_num = 360.0 scan_msg.angle_max = self.angle_max # LS-lidar rotates clockwise, which is disobey right-hand rules for coordinate system scan_msg.angle_min = self.angle_min scan_msg.header.frame_id = 'base_laser_link' scan_msg.angle_increment = 2 * math.pi / point_num scan_msg.range_min = 0.15 scan_msg.range_max = 3.0 scan_msg.header.stamp = rospy.Time.now() # print("begin to select") if (self.angle_min > 0) or (self.angle_max <= 0): range_part = laser_distance1[(self.angle_range >= self.angle_min) & (self.angle_range <= self.angle_max)] scan_msg.ranges = np.flipud(range_part) else: range_part1 = laser_distance1[(self.angle_range >= self.angle_min) & (self.angle_range <= 0)] range_part2 = laser_distance1[(self.angle_range <= self.angle_max) & (self.angle_range > 0)] p1 = np.flipud(range_part1) p2 = np.flipud(range_part2) scan_msg.ranges = np.append(p1, p2) scan_msg.intensities = [255] * len(laser_distance1) pub.publish(scan_msg)
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 timer_callback(self, timer): ts = rospy.Time.now() # pub scan scan = LaserScan() scan.header.stamp = ts scan.header.frame_id = 'ego_racecar/laser' scan.angle_min = self.angle_min scan.angle_max = self.angle_max scan.angle_increment = self.angle_inc scan.range_min = 0. scan.range_max = 30. scan.ranges = self.ego_scan self.ego_scan_pub.publish(scan) scan = LaserScan() scan.header.stamp = ts scan.header.frame_id = 'ego_racecar/laser' scan.angle_min = self.angle_min scan.angle_max = self.angle_max scan.angle_increment = self.angle_inc scan.range_min = 0. scan.range_max = 30. scan.ranges = self.opp_scan self.opp_scan_pub.publish(scan) # pub tf self.publish_odom(ts) self.publish_transforms(ts) self.publish_laser_transforms(ts) self.publish_wheel_transforms(ts) # pub race info self.publish_race_info(ts)
def callback(sub,sub5): l = [sub.ranges,sub5.range] ir = sub5.range #print sub.ranges[320], "The Laser Scan Data" #print ir, "The IR sensor Data" pub = rospy.Publisher('scan2', LaserScan, queue_size=10) #rospy.init_node('laser_scan_publisher') scan_pub = rospy.Publisher('uvbot_laser_scan', LaserScan, queue_size=10) num_readings = 720 laser_frequency = 60 count = 0 r = rospy.Rate(1.0) while not rospy.is_shutdown(): current_time = rospy.Time.now() scan = LaserScan() scan.header.stamp = current_time scan.header.frame_id = 'hokuyo' scan.angle_min = -1.57 scan.angle_max = 1.57 scan.angle_increment = 3.14 / num_readings scan.time_increment = (1.0 / laser_frequency) / (num_readings) scan.range_min = 0.1 scan.range_max = 10.0 #l = [] #for i in range(720): #l.append(sub.ranges[i]) #scan.ranges = l scan.ranges = sub.ranges if sub5.range < 0.1: for i in range(320,340): a = sub5.range scan.ranges[i] = a scan.intensities = [] else: scan.ranges = sub.ranges scan.intensities = sub.intensities #for i in range(0, 100): #if i<=50: #scan.ranges.append(100) #else: #scan.ranges.append(20) print len(scan.ranges) scan_pub.publish(scan) count += 1 r.sleep()
def laser_dummyfile(shift_rad=0): lidardummymsg = readlidardummy() scanmsg = LaserScan() scanmsg.header.frame_id = lidardummymsg['header']['frame_id'] scanmsg.header.seq = 0 scanmsg.angle_min = 0 + shift_rad scanmsg.angle_max = 3.14 * 2 + shift_rad #360 deg scanmsg.range_min = 0 scanmsg.range_max = 50 scanmsg.angle_increment = lidardummymsg['angle_increment'] shiftptnum = int(shift_rad / scanmsg.angle_increment) scanmsg.ranges = lidardummymsg['ranges'] scanmsg.ranges = np.roll(scanmsg.ranges, -shiftptnum) return scanmsg
def _test(self, N_scan, N_part): # build a laser scan of a square dead end scan = LaserScan( angle_min=-np.pi/2, angle_increment=np.pi/(N_scan - 1), range_max=10, range_min=0 ) angles = scan.angle_min + np.arange(N_scan)*scan.angle_increment scan.ranges = 5 * np.where(np.abs(angles) < np.pi/4, 1/np.cos(angles), 1/np.abs(np.sin(angles))) # now create some particles: particles = np.recarray(N_part + 1, dtype=mcl.particle_filter.pose_dtype) particles[:-1].x = np.random.normal(loc=0, scale=1, size=N_part) particles[:-1].y = np.random.normal(loc=0, scale=1, size=N_part) particles[:-1].theta = np.random.normal(loc=0, scale=np.pi/8, size=N_part) particles[-1] = (0, 0, 0) start = time.time() print "Started sensor matching" weights = mcl.ros_sensor_update.sensor_update(self.map, scan, particles, np.eye(4)) print 'Took', time.time() - start if len(weights) < 20: print weights self.assertEqual(np.argmax(weights), len(particles) - 1, "The most likely particle should be at the origin")
def tfmini_laserscan_publisher(frame_id): scan_pup = rospy.Publisher('tfmini_laser', LaserScan, queue_size=0) scan = LaserScan() #-- Convention: counter clockwise is positive (left positive, right negative) tfminiscanner = TfminiServoScanner(SERVO_GPIO, SRV_ANGLE_MIN, SRV_ANGLE_MAX, SRV_DUTY_ANGLE_MIN, SRV_DUTY_ANGLE_MAX, LASER_ANGLE_SAMPLES, SRV_TIME_MIN_MAX) #-- Initialize the message scan.header.frame_id = frame_id scan.range_min = tfminiscanner.laser.distance_min * 0.01 scan.range_max = tfminiscanner.laser.distance_max * 0.01 tfminiscanner.reset_servo() time.sleep(1) counter = 0 while not rospy.is_shutdown(): ini_angle, end_angle, time_increment, angle_increment, ranges = tfminiscanner.scan( scale_factor=0.01, reset=True) scan.angle_min = ini_angle scan.angle_max = end_angle scan.angle_increment = angle_increment scan.time_increment = time_increment scan.ranges = ranges scan_pup.publish(scan)
def prepare_laserscan_msg(self): ''' Fill laser scan message ''' laser_scan_msg = LaserScan() #Step 1: num_readings = 100 laser_frequency = 40 ranges = [] intensities = [] count = 0 i = 0 #generate some fake data for laser scan while (i < num_readings): ranges.append(count) intensities.append(4 + count) i = i + 1 #Step 2 now = rospy.get_rostime() laser_scan_msg.header.stamp = now laser_scan_msg.header.frame_id = "laser_frame" laser_scan_msg.angle_min = -1.57 laser_scan_msg.angle_max = 1.57 laser_scan_msg.angle_increment = 3.14 / num_readings laser_scan_msg.time_increment = (1 / laser_frequency) / (num_readings) laser_scan_msg.range_min = 0.0 laser_scan_msg.range_max = 4.0 laser_scan_msg.ranges = ranges laser_scan_msg.intensities = intensities
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 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 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 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 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 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 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 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 handleScanner(self): scan = LaserScan() scan.header.stamp = self.current_time self.ser.write("0") line = self.ser.readline() if len(line) <= 3: return direction = line[0:2] if direction == '+:': self.direction = 1.0 elif direction == '-:': self.direction = -1.0 else: return rospy.logdebug(line) scan.header.frame_id = 'laser_frame' scan.angle_min = -1.57 * self.direction scan.angle_max = 1.57 * self.direction scan.angle_increment = 3.14 / self.num_readings * self.direction scan.time_increment = 0.003012 * self.num_readings scan.range_min = 0.02 # [m] scan.range_max = 4.00 # [m] scan.ranges = map(lambda n: 1.0 * int(n) / 1000, line[2:].split(',')) self.scan_pub.publish(scan)
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 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 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 to_laserscan(self, frame): """Returns a LaserScan message correspon2ding to slice. Args: frame: Frame ID. Returns: A sensor_msgs.msg.LaserScan. """ scan = LaserScan() scan.header.frame_id = frame scan.header.stamp = self.timestamp scan.angle_min = self.angle_min scan.angle_max = self.angle_max scan.angle_increment = self.step #scan.ranges = self.bins scan.ranges = [b * 1.45 / 2000 for b in self.bins] # points out if this range are discarded scan.range_min = 0.0 scan.range_max = 70000.0 return scan
def fillUpOldLaserMessage(words): laser_msg = LaserScan() laser_msg.header.frame_id = "base_link" ranges = [] num_range_readings = int(words[1]) last_range_reading = num_range_readings + 1 for word in words[2:last_range_reading + 1]: ranges.append(float(word)) ang_range = float(180) # Get angular resolution ang_res = (radians(ang_range) / (num_range_readings - 0.0)) # Get max reading max_reading = 20. # init laser msg ang_min = radians(-ang_range / 2.) ang_max = radians(+ang_range / 2.) - ang_res laser_msg.angle_min = ang_min laser_msg.angle_max = ang_max laser_msg.angle_increment = ang_res laser_msg.range_min = 0 laser_msg.range_max = max_reading laser_msg.ranges = ranges laser_msg.header.stamp = rospy.Time(float(words[last_range_reading + 7])) return laser_msg
def handle_scanner_msg(self, args): # Broadcast scanner transform first time_now = rospy.Time.now() pos = self.laser_tf.transform.translation rot = self.laser_tf.transform.rotation self.tf_broadcaster.sendTransform((pos.x, pos.y, pos.z), (rot.x, rot.y, rot.z, rot.w), time_now, self.laser_tf.child_frame_id, self.laser_tf.header.frame_id) scan = LaserScan() scan.header.stamp = time_now scan.header.frame_id = self.base_laser_frame for (name, par) in args.items(): if name == 'Range': scan.ranges = [float(s) for s in par.split(',')] if name == 'FOV': fov = float(par) scan.angle_min = -fov/2.0 scan.angle_max = fov/2.0 if name == 'Resolution': scan.angle_increment = float(par) scan.range_min = 0.0 scan.range_max = 20.0 self.scan_pub.publish(scan)
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 getLaserScan(frame_id, laser_scan_line): # Timestamp [seconds.microseconds] # # of ranges [unitless] # Angular offset [1/4 degree] # R1..R181 Ranges (zero padded to 181 ranges) [m] # # 1235603336.30835, 181, 0, 11.360, 11.360, 11.390, 11.410, 81.910, 81.910, 11.380, 11.400, 11.430, 6.450, 6.170, 6.030, 5.880, 5.740, 5.600, 5.470, 5.360, 5.370, 5.390, 5.430, 5.470, 5.500, 5.530, 5.580, 5.610, 5.410, 5.230, 5.130, 5.180, 5.230, 5.280, 5.350, 6.040, 6.110, 6.180, 6.250, 6.330, 6.400, 6.490, 5.950, 5.750, 5.640, 5.520, 5.440, 5.330, 5.220, 5.280, 5.040, 5.490, 5.590, 5.690, 5.810, 5.930, 6.080, 6.210, 6.360, 6.530, 6.690, 6.870, 13.930, 13.770, 13.650, 13.650, 13.530, 13.430, 13.300, 13.190, 13.040, 12.870, 12.780, 12.700, 12.630, 12.550, 12.480, 12.410, 12.360, 12.310, 12.240, 12.200, 12.150, 12.110, 12.070, 12.040, 12.010, 11.990, 11.970, 11.560, 11.930, 11.920, 11.920, 11.910, 11.930, 11.920, 11.920, 11.940, 11.930, 12.830, 12.840, 12.300, 12.130, 12.120, 13.000, 12.250, 12.230, 12.270, 12.330, 12.390, 12.440, 12.520, 12.580, 12.810, 13.640, 13.740, 13.830, 13.940, 13.640, 6.410, 6.220, 6.010, 5.810, 5.640, 5.080, 4.180, 4.090, 4.250, 4.070, 4.050, 3.700, 3.560, 3.510, 3.510, 3.570, 3.430, 3.520, 3.590, 4.940, 4.650, 4.630, 5.050, 5.040, 5.080, 4.890, 2.790, 2.710, 2.660, 2.620, 2.590, 2.600, 2.660, 2.650, 2.630, 2.690, 2.790, 2.900, 4.250, 4.150, 2.510, 2.480, 2.390, 2.360, 2.330, 2.320, 2.300, 2.410, 2.270, 3.930, 2.290, 2.390, 3.850, 3.830, 3.830, 3.710, 4.060, 4.050, 4.040, 4.030, 4.020, 4.010, 4.010, 4.010, 4.010 str_timestamp, str_num_readings, str_angular_offset, str_ranges = laser_scan_line.split(', ', 3) num_readings = int(str_num_readings) angular_offset = float(str_angular_offset) ranges = map(float, str_ranges.split(', ')) #convert array of readings laser_frequency = 50 angle_range_rad = 180 * np.pi / 180 #populate the LaserScan message msg = LaserScan() msg.header.stamp = rospy.Time.now() msg.header.frame_id = frame_id msg.angle_min = - (angle_range_rad / 2) msg.angle_max = (angle_range_rad / 2) msg.angle_increment = angle_range_rad / num_readings msg.time_increment = (1 / laser_frequency) / (num_readings) msg.range_min = 0.0 msg.range_max = 40.0 msg.ranges = ranges msg.intensities = [0.0] * len(ranges) return msg
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 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 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 republish_laser_scan(self, data, fov_adjustment=1.0, publisher=None, combined_publisher=None): """Publish range as laser scan.""" if not self._running: return laser_scan_message = LaserScan() laser_scan_message.range_max = data.max_range laser_scan_message.range_min = data.min_range laser_scan_message.angle_min = -(data.field_of_view / (2 / fov_adjustment)) laser_scan_message.angle_max = data.field_of_view / (2 / fov_adjustment) laser_scan_message.angle_increment = data.field_of_view / ( 2 / fov_adjustment) laser_scan_message.ranges = [data.range] * 3 laser_scan_message.header.frame_id = data.header.frame_id laser_scan_message.header.stamp = data.header.stamp if publisher is not None: publisher.publish(laser_scan_message) if combined_publisher is not None: combined_publisher.publish(laser_scan_message)
def read(pub): if (ser.in_waiting >= 9): a = rospy.Time.now() # print("read") Dist_Total = -1 if ((b'Y' == ser.read()) and (b'Y' == ser.read())): Dist_L = ser.read() Dist_H = ser.read() Strength_L = ser.read() Strength_H = ser.read() Strength = ord(Strength_L) + 256 * ord(Strength_H) Mode = ser.read() tmp = ser.read() CheckSum = ser.read() Dist_Total = (ord(Dist_H) * 256) + (ord(Dist_L)) scan = LaserScan() # print(rospy.Time.now(),Dist_Total) scan.header.stamp = a scan.header.frame_id = 'laser_frame' scan.angle_min = 0. scan.angle_max = 0. scan.angle_increment = 0 scan.time_increment = 0 scan.range_min = 30. scan.range_max = 1200. scan.ranges = [] scan.intensities = [] scan.ranges.append(Dist_Total) scan.intensities.append(Strength) pub.publish(scan)
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 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 talker(): pub = rospy.Publisher('scan', LaserScan, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz br = tf.TransformBroadcaster() sequence = 1 while not rospy.is_shutdown(): br.sendTransform((0.1, 0.1, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), 'base_laser', 'base_link') br.sendTransform( (sequence * 0.01, sequence * 0.01, 0), tf.transformations.quaternion_from_euler(0, 0, sequence * 0.01), rospy.Time.now(), 'base_link', 'odom') scan = LaserScan() scan.header.seq = sequence scan.header.stamp = rospy.get_rostime() scan.header.frame_id = 'base_laser' scan.angle_min = -0.1 scan.angle_max = 0.1 scan.angle_increment = 0.1 scan.range_max = 5.0 scan.ranges = [4.0, 4.0, 4.0] scan.intensities = [0.1, 0.1, 0.1] rospy.logdebug(scan) pub.publish(scan) sequence += 1 rate.sleep()
def callback(data): n = int(rospy.get_param('~reduction', '2')) pub=rospy.Publisher('/scan_reduced', LaserScan) newScan=LaserScan() newScan=data newScan.angle_increment=n*data.angle_increment newScan.ranges=data.ranges[1::n] pub.publish(newScan)
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 publish_laser_msg(self, ranges): msg = LaserScan() msg.angle_min = self.robot.laser.min_angle msg.angle_max = self.robot.laser.max_angle msg.angle_increment = self.robot.laser.resolution msg.range_min = 0.0 msg.range_max = self.robot.laser.range msg.ranges = ranges msg.header.stamp = rospy.Time.now() self.laser_publisher.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 make_wallscan(self, data): num_readings = len(data) wall_scan = LaserScan() wall_scan.header.frame_id = "base_laser_link" wall_scan.ranges = data wall_scan.angle_min = -3.14; wall_scan.angle_max = 3.14; wall_scan.angle_increment = (3.14*2) / num_readings; wall_scan.range_min = 0.0; wall_scan.range_max = 5; return wall_scan
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 publishLaserScan(self,data=None): scan = LaserScan() scan.header.seq = 1 scan.header.stamp = rospy.Time.now() num_readings = 100 laser_frequency = 40 scan.header.frame_id = "base_laser" scan.angle_min = radians(-30) scan.angle_max = radians(30) scan.angle_increment = radians(60) / num_readings scan.time_increment = (1 / laser_frequency) / (num_readings) scan.range_min = 0.5 scan.range_max = 6 scan.ranges = [5]*num_readings self.laserScanPublisher.publish(scan)
def Cb(self, msg): n_ranges = list() pv = msg.data[0] for i in range(50): if pv>0.0 and msg.data[i]>0.0: n_ranges.append((pv+msg.data[i])/2) else: n_ranges.append(0.0) n_ranges.append(msg.data[i]) pv = msg.data[i] laserMsg = LaserScan() laserMsg.ranges = n_ranges laserMsg.angle_increment = 0.2; laserMsg.time_increment = 1/50 laserMsg.header.frame_id = '/ultra' laserMsg.header.stamp = rospy.Time.now() self.scanPub.publish(laserMsg)
def handle_laser_scan(laserMsg,currTime): scan = LaserScan() scan.header.stamp = currTime scan.header.frame_id = "base_link" scan.angle_min = -1.57 scan.angle_max = 1.57 scan.angle_increment = 3.14/181 scan.time_increment = (1/0.5)/181 scan.range_min = 0.0 scan.range_max = 10.0 scan.ranges = [0.0 for i in xrange(len(laserMsg)-1)] scan.intensities = [0.0 for i in xrange(len(laserMsg)-1)] for i in xrange(len(laserMsg)-1): scan.ranges[i] =laserMsg[i] scan.intensities[i]= 100 laserPub = rospy.Publisher('scan',LaserScan,queue_size = 1) laserPub.publish(scan) rospy.sleep(4.0)
def __init__(self): rospy.init_node('fake_laser') # parameters self.rate = rospy.get_param("~rate", 1.0) self.frame_id = rospy.get_param("~frame", "base_laser") self.range_min = rospy.get_param("~range_min", 0.2) self.range_max = rospy.get_param("~range_max", 5.5) self.angle_min = rospy.get_param("~angle_min", -1.57) self.angle_max = rospy.get_param("~angle_max", 1.57) self.num_readings = rospy.get_param("~num_readings", 100) self.fixed_reading = rospy.get_param("~fixed_reading", 2.0) self.scan_time = rospy.get_param("~scan_time", 0.1) self.angle_increment = (self.angle_max - self.angle_min) / self.num_readings self.time_increment = self.scan_time / (self.num_readings) self.scanPub = rospy.Publisher('scan', LaserScan) rospy.loginfo("Started fake laser node publishing to /scan topic.") r = rospy.Rate(self.rate) while not rospy.is_shutdown(): ranges = list() # Generate the fake data. for i in range(self.num_readings): ranges.append(self.fixed_reading * sin(i) * random.uniform(0, 1)) scan = LaserScan() scan.header.stamp = rospy.Time.now() scan.header.frame_id = self.frame_id scan.angle_min = self.angle_min scan.angle_max = self.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 = ranges self.scanPub.publish(scan) r.sleep()
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 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 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 publish_laser_data(self, publisher): scan = LaserScan() scan.header.stamp = rospy.Time.now() scan.header.frame_id = "base_laser_link" scan.angle_min = -30 * math.pi / 180 scan.angle_max = 30 * math.pi / 180 scan.angle_increment = 1 * math.pi / 180 scan.range_min = 0.03 scan.range_max = 3.00 scan.ranges = [] # read the ranges from left to right for i in range(0,15): scan.ranges.append(self.ranges["ultrasonic_3"]) for i in range(15,30): scan.ranges.append(self.ranges["ultrasonic_1"]) for i in range(30,45): scan.ranges.append(self.ranges["ultrasonic_2"]) for i in range(45,60): scan.ranges.append(self.ranges["ultrasonic_4"]) publisher.publish(scan) # clear out the scans self.resetRanges()
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 _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 generate_laser_msg(self, laser_data): '''Generates a 'LaserScan' message from the input data. Keyword arguments: laser_data -- A 'LaserData' object. Returns: msg -- A 'LaserScan' message. ''' laser_scan_msg = LaserScan() laser_scan_msg.header.stamp = rospy.Time.now() laser_scan_msg.header.frame_id = laser_data.frame_id laser_scan_msg.angle_min = self.scanner_min_angle laser_scan_msg.angle_max = self.scanner_max_angle laser_scan_msg.angle_increment = self.scanner_angle_increment laser_scan_msg.time_increment = self.scanner_time_increment laser_scan_msg.range_min = self.scanner_min_range laser_scan_msg.range_max = self.scanner_max_range laser_scan_msg.ranges = self.calculate_ranges(laser_data) return laser_scan_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 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(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)
def spin(self): encoders = [0,0] self.x = 0 # position in xy plane self.y = 0 self.th = 0 then = rospy.Time.now() # things that don't ever change scan_link = rospy.get_param('~frame_id','base_laser_link') scan = LaserScan(header=rospy.Header(frame_id=scan_link)) scan.angle_min = 0 scan.angle_max = 6.26 scan.angle_increment = 0.017437326 scan.range_min = 0.020 scan.range_max = 5.0 odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_link') # main loop of driver r = rospy.Rate(5) self.robot.requestScan() while not rospy.is_shutdown(): # prepare laser scan scan.header.stamp = rospy.Time.now() #self.robot.requestScan() scan.ranges = self.robot.getScanRanges() # get motor encoder values left, right = self.robot.getMotors() # get analog sensors self.robot.getAnalogSensors() # get drop sensors left_drop = self.robot.state["LeftDropInMM"] right_drop = self.robot.state["RightDropInMM"] # send updated movement commands self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1]))) # ask for the next scan while we finish processing stuff self.robot.requestScan() # now update position information dt = (scan.header.stamp - then).to_sec() then = scan.header.stamp d_left = (left - encoders[0])/1000.0 d_right = (right - encoders[1])/1000.0 encoders = [left, right] dx = (d_left+d_right)/2 dth = (d_right-d_left)/(BASE_WIDTH/1000.0) 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 = rospy.Time.now() 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 # prepare drop drop = NeatoDropSensor() drop.header.stamp = rospy.Time.now() drop.left = left_drop drop.right = right_drop # publish everything self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), then, "base_link", "odom" ) self.scanPub.publish(scan) self.odomPub.publish(odom) self.dropPub.publish(drop) # wait, then do it again r.sleep() # shut down self.robot.setLDS("off") self.robot.setTestMode("off")
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])) 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)