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 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 _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 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 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 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 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 _create_scan(self): scan = LaserScan() scan.header.frame_id = self._frame_id scan.header.stamp = rospy.get_rostime() scan.range_min = self._range_min scan.range_max = self._range_max scan.angle_min = numpy.deg2rad(self._angle_min) scan.angle_max = numpy.deg2rad(self._angle_max) scan.angle_increment = numpy.deg2rad(self._angle_increment) scan.scan_time = self._scan_time scan.time_increment = self._time_increment scan.ranges = [self._r] * self._num_readings scan.intensities = [1.0] * self._num_readings return 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 publishLaserScan(self,data=None): scan = LaserScan() scan.header.seq = 1 scan.header.stamp = rospy.Time.now() num_readings = 100 laser_frequency = 20 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 = 10 scan.ranges = [8]*num_readings self.laserScanPublisher.publish(scan)
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 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 send_laser_scan(self, scans): self.seq += 1 laserScan = LaserScan() laserScan.header.seq = self.seq laserScan.header.stamp = rospy.Time.now() laserScan.header.frame_id = "base_link" laserScan.angle_min = -self.lidar_fov / 2.0 laserScan.angle_max = self.lidar_fov / 2.0 laserScan.angle_increment = 1.0 * np.pi / 180.0 laserScan.time_increment = 0.00001 laserScan.scan_time = 0.001 * 181 laserScan.range_min = 0.0 laserScan.range_max = self.z_max laserScan.ranges = scans laserScan.intensities = [] self.scan_pub.publish(laserScan)
def callback(self,_data): _ls=LaserScan() _ls.header.frame_id="laser" _ls.angle_min=-3.14 _ls.angle_max=+3.14 _ls.angle_increment=6.28/360.0 _ls.time_increment=0.1 _ls.range_min=_data.range_min _ls.range_max=_data.range_max for i in range(0,len(_data.ranges)): _ls.ranges.append(2) for i in range(0, len(_data.intensities)): _ls.intensities.append(3) self.pub_laser.publish(_ls)
def pub_laserscan(obs_distance): laserscan = LaserScan() laserscan.header.stamp = rospy.Time.now() laserscan.header.frame_id = 'lidar' laserscan.angle_min = -1.57 laserscan.angle_max = 1.57 # 对应180度 laserscan.angle_increment = 3.14 / 180 #弧度的增量,这样就是隔1度取值 laserscan.time_increment = 1.0 / 10 / 180 # 中间的10对应于json中的RotationsPerSecond laserscan.range_min = 0.0 laserscan.range_max = 100.0 laserscan.ranges = [] # 距离 laserscan.intensities = [] # 强度 for i in range(1,len(obs_distance)): laserscan.ranges.append(obs_distance[i]) laserscan.intensities.append(0.0) print(laserscan) return laserscan
def make_samples(self): ls = LaserScan() ls.header.stamp = rospy.Time.now() ls.header.frame_id = "laser" ls.angle_min = -3.14 ls.angle_max = 3.14 ls.angle_increment = 6.28 / 360.0 ls.time_increment = 0.1 ls.range_min = 0.0 ls.range_max = 100.0 ls.ranges = [0.7 for i in range(0, 360)] # _lt1 = [i * 0.002 for i in range(0, 180)] # _lt2 = [(i - 180) * 0.002 for i in range(360, 180, -1)] # ls.ranges = _lt1 + _lt2 self.pub_sample.publish(ls)
def publish_scan(self,msg): pub = rospy.Publisher('%s/scan'%(subscribed_robot), LaserScan, queue_size=1000) #rate = ros # py.Rate(10) #while not rospy.is_shutdown(): scan_msg = LaserScan() scan_msg.header.frame_id = "%s/base_scan"%(subscribed_robot) scan_msg.angle_increment= msg["angle_increment"] scan_msg.angle_max = msg["angle_max"] scan_msg.angle_min = msg["angle_min"] scan_msg.range_max = msg["range_max"] scan_msg.range_min = msg["range_min"] scan_msg.time_increment = msg["time_increment"] scan_msg.ranges = msg["ranges"] scan_msg.intensities = msg["intensities"] pub.publish(scan_msg) rospy.logdebug("ROS published to {}/scan".format(subscribed_robot))
def publish_filtered_laser_scan(self, laser_original_data, new_filtered_laser_range, pub_num): 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 laser_distance = 0.0 else: laser_distance = item laser_filtered_object.ranges.append(laser_distance) laser_filtered_object.intensities.append(item) if pub_num == '1': self.laser_filtered_pub_1.publish(laser_filtered_object) elif pub_num == '2': self.laser_filtered_pub_2.publish(laser_filtered_object) elif pub_num == '3': self.laser_filtered_pub_3.publish(laser_filtered_object)
def callback(arg): laser = LaserScan() laser.header = arg.header # timestamp in the header is the acquisition time on laser.angle_min = arg.angle_min # start angle of the scan [rad] laser.angle_max = arg.angle_max # end angle of the scan [rad] laser.angle_increment = arg.angle_increment # angular distance between measurements [rad] laser.time_increment = arg.time_increment # time between measurements [seconds] - if your scanne laser.scan_time = arg.scan_time # time between scans [seconds] laser.range_min = arg.range_min # minimum range value [m] laser.range_max = arg.range_max # maximum range value [m] for i in range(len(arg.ranges)): laser.ranges.append(arg.ranges[i]) rospy.sleep(1) pub.publish(laser)
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 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 front_laser_callback(laser_msg): pub = rospy.Publisher('/fused_laser', LaserScan, queue_size=1) scan = LaserScan() scan.header.stamp = laser_msg.header.stamp scan.header.frame_id = "front_laser" scan.angle_min = -1.57 scan.angle_max = 1.57 scan.angle_increment = 3.14 / 180 scan.time_increment = (1.0 / 75.0) / 181 scan.range_min = 0.0 scan.range_max = 30.0 scan.ranges = laser_msg.ranges scan.intensities = laser_msg.intensities pub.publish(scan)
def callback(self, data): laser = LaserScan() laser.header = data.header 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.scan_time = data.scan_time laser.range_min = data.range_min laser.range_max = data.range_max laser.intensities = data.intensities laser.ranges = data.ranges cloud = self.laserProj.projectLaser(laser) self.pub.publish(cloud)
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 build_laser_scan(self, ranges): arc = math.pi / 2 # Laser arc 45 to 135 = 90 deg (in laser coordinate frame -45 45) num_readings = 181 # number of readings of the laser (len(temp_data)) laser_frequency = 5 result = LaserScan() result.header.stamp = rospy.Time.now() result.header.frame_id = "laser_frame" result.angle_min = -0.785398 # math.PI/4 -45 degree result.angle_max = 0.785398 #math.PI/4 45 degree result.angle_increment = arc / num_readings # 5 degree result.time_increment = (1 / laser_frequency) / (num_readings) result.range_min = 0.0 result.range_max = 26.0 result.ranges = ranges return result
def callback2(self, arg): laser = LaserScan() laser.header = arg.header laser.angle_min = arg.angle_min laser.angle_max = arg.angle_max laser.angle_increment = arg.angle_increment laser.time_increment = arg.time_increment laser.scan_time = arg.scan_time laser.range_min = arg.range_min laser.range_max = arg.range_max laser.intensities = arg.intensities laser.ranges = arg.ranges cloud = self.laserProj.projectLaser(laser) self.pub2.publish(cloud)
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 callback(msg): new_msg = LaserScan() # Make a new message, and copy in the information new_msg.header = msg.header new_msg.angle_min = msg.angle_min new_msg.angle_max = msg.angle_max new_msg.angle_increment = msg.angle_increment new_msg.time_increment = msg.time_increment new_msg.scan_time = msg.scan_time new_msg.range_min = msg.range_min new_msg.range_max = msg.range_max new_msg.intensities = msg.intensities[:] # Noise up the ranges new_msg.ranges = [gauss(r, 0.1) for r in msg.ranges] pub.publish(new_msg)
def GetScanInRange(self, scan, angle_min, angle_max): scan_filtered = LaserScan() scan_filtered.header = scan.header scan_filtered.angle_increment = scan.angle_increment scan_filtered.range_max = scan.range_max scan_filtered.range_min = scan.range_min scan_filtered.scan_time = scan.scan_time scan_filtered.time_increment = scan.time_increment ## Round the input range to the closest valid angle num_pts = len(scan.ranges) - 1 min_idx = int( round((angle_min - scan.angle_min) / scan.angle_increment)) if min_idx < 0: print("Warning: angle_min is less than minimum scan angle.") min_idx = 0 elif min_idx > num_pts: print("Warning: angle_min is less than maximum scan angle.") min_idx = num_pts angle_min = scan.angle_min + min_idx * scan.angle_increment max_idx = int( round((angle_max - scan.angle_min) / scan.angle_increment)) if max_idx < 0: print("Warning: angle_max is less than minimum scan angle.") max_idx = 0 elif max_idx > num_pts: print("Warning: angle_max is less than maximum scan angle.") max_idx = num_pts angle_max = scan.angle_min + max_idx * scan.angle_increment ## Output the final values scan_filtered.angle_min = angle_min scan_filtered.angle_max = angle_max scan_filtered.ranges = scan.ranges[min_idx:max_idx] """ for i in range(len(scan.ranges)): angle = angle_min + angle_inc*i angle_deg = math.degrees(angle) print("Angle: {:3.2f} deg, Range: {:2.2f}".format(angle_deg, scan.ranges[i])) """ return scan_filtered
def publish_tof(self, distances): msg = Float32MultiArray(data=distances) self._pub_tof.publish(msg) scan = LaserScan() scan.header.stamp = self._timestamp scan.header.frame_id = "laser" scan.angle_min = self._angle_min scan.angle_max = self._angle_max scan.angle_increment = self._angle_inc scan.time_increment = 1.0 / 50.0 scan.range_min = 0.0 scan.range_max = self._rng_tof scan.ranges = [] scan.intensities = [] for i in range(0, self._laserbeams): scan.ranges.append(distances[i]) scan.intensities.append(1) self._pub_laser.publish(scan)
def scan_invert(self, scan): current_time = rospy.Time.now() new_scan = LaserScan() new_scan.header.stamp = scan.header.stamp new_scan.header.frame_id = scan.header.frame_id new_scan.angle_min = scan.angle_min new_scan.angle_max = scan.angle_max new_scan.angle_increment = scan.angle_increment new_scan.time_increment = scan.time_increment new_scan.range_min = scan.range_min new_scan.range_max = scan.range_max new_scan.ranges = scan.ranges[::-1] new_scan.intensities = scan.intensities[::-1] #print(new_scan.ranges) self.scan_pub.publish(new_scan)
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 publish_filtered_laser_scan(self): length_range = len(self.laser_scan.ranges) length_intensities = len(self.laser_scan.intensities) #index = int(length_range / 2) #self.index = 10 self.index += 1 if self.index >= length_range - 1: self.index = 0 print str(self.index) wanted_value = self.laser_scan.ranges[self.index] #wanted_value = 1.0 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 = self.laser_scan.angle_min laser_filtered_object.angle_max = self.laser_scan.angle_max laser_filtered_object.angle_increment = self.laser_scan.angle_increment laser_filtered_object.time_increment = self.laser_scan.time_increment laser_filtered_object.scan_time = self.laser_scan.scan_time laser_filtered_object.range_min = self.laser_scan.range_min laser_filtered_object.range_max = self.laser_scan.range_max laser_filtered_object.ranges = [self.laser_scan.range_min ] * length_range """ laser_filtered_object.ranges = [] for item in self.laser_scan.ranges: laser_filtered_object.ranges.append(item) """ laser_filtered_object.intensities = [100.0] * length_intensities laser_filtered_object.ranges[self.index] = wanted_value self.laser_filtered_pub.publish(laser_filtered_object)
def handleRange(self, data): """Callback to receive range data.""" noise = random.gauss(0.0, self.config.noise_stdev) if data.range > self.config.max_range: self.sonar_range_ = self.config.max_range + 2 else: self.sonar_range_ = data.range + noise self.sonar_range_available_ = True if self.joint_state_available_: # Publish beam msg = LaserScan() msg.header = data.header msg.header.frame_id = self.config.frame_id msg.angle_increment = 0 idx = self.joint_state_.name.index(self.jointname) msg.angle_min = self.joint_state_.position[idx] msg.angle_max = self.joint_state_.position[idx] msg.time_increment = 0 msg.range_min = 1.0 msg.range_max = self.config.max_range bindist = msg.range_max / self.config.nbins msg.ranges = np.arange(0 + bindist, msg.range_max + 0.000001, bindist).tolist() msg.intensities = [0] * self.config.nbins if self.sonar_range_ <= self.config.max_range: index = int(round(self.sonar_range_ / bindist)) for i in range(-2, 3): j = index + i if j > 0 and j < len(msg.intensities): msg.intensities[j] = 255 self.tf_broadcaster_.sendTransform( (self.translation[0], self.translation[1], self.translation[2]), tf.transformations.quaternion_from_euler( self.rotation_rpy[0], self.rotation_rpy[1], self.rotation_rpy[2]), msg.header.stamp, self.config.frame_id, self.config.robot_frame_name) self.sonar_beam_pub_.publish(msg) # adabt range to binning self.sonar_range_ = int(round(data.range / bindist)) * bindist
def generate(scan_rate, angle_min, angle_max, ranges, intensities): scan = LaserScan() start_time = rospy.Time.now() angle_increment = (angle_max - angle_min) / 400 time_increment = 1 / scan_rate scan.header.stamp = start_time scan.header.frame_id = 'scanner' scan.angle_min = angle_min scan.angle_max = angle_max scan.angle_increment = angle_increment scan.time_increment = time_increment scan.range_min = 0 scan.range_max = 10 scan.ranges = ranges scan.intensities = intensities return scan
def init(): rospy.init_node('laserscan_spoofer') pub = rospy.Publisher('laserscan_spoofer/scan', LaserScan, queue_size=1) rate = rospy.Duration(0.1) msg = LaserScan() msg.header.frame_id = 'laser_link' msg.angle_min = -2.35619449615 msg.angle_max = 2.09234976768 msg.angle_increment = 0.00613592332229 msg.time_increment = 0.0000976562732831 msg.scan_time = 0.10000000149 msg.range_min = 0.019999999553 msg.range_max = 5.59999990463 sz = int(float(msg.angle_max - msg.angle_min) / float(msg.angle_increment)) msg.ranges = [float('NaN') for i in range(sz)] while not rospy.is_shutdown(): msg.header.stamp = rospy.Time(0) pub.publish(msg) rospy.sleep(rate)
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 test_angular_bounds_filter(): """ Test angular bounds filter """ print('\n===== Testing angular bounds filter =====\n') # generate toy laser scan num_readings = 20 laser_frequency = 40 scan = LaserScan() scan.header.stamp = 0 scan.header.frame_id = 'laser_frame' scan.angle_min = 0 scan.angle_max = num_readings scan.angle_increment = 1.0 scan.time_increment = (1.0 / laser_frequency) / (num_readings) scan.range_min = 0.0 scan.range_max = 100.0 scan.ranges = [] scan.intensities = [] for i in range(0, num_readings): scan.ranges.append(scan.angle_min + i * scan.angle_increment) # fake data scan.intensities.append(1) # fake data print('\nBefore filter:\n', scan) # test filter lower_angle = 6 upper_angle = 18 print('*** Test: lower angle: {}, upper angle: {} ***'.format(lower_angle, upper_angle)) angular_bounds_filter(scan, lower_angle, upper_angle) print('\nAfter angular bounds filter:\n', scan) assert scan.angle_min == lower_angle assert scan.angle_max == upper_angle assert scan.ranges[0] == lower_angle assert scan.ranges[-1] == upper_angle assert len(scan.ranges) == len(scan.intensities) == upper_angle - lower_angle + 1 for i in range(len(scan.ranges) - 1): assert scan.ranges[i + 1] - scan.ranges[i] == scan.angle_increment
def rec_data(self): try: self.rdata = self.sock.recv(self.data_size) if not self.rdata: print("broken connection") self.disconnect() print("length of rdata :",len(self.rdata)) #print(self.rdata) rospy.loginfo("rec_data") scan = LaserScan() scan.header.frame_id = 'laser' scan_time = rospy.Time.now() scan.header.stamp = scan_time scan.angle_min = 0 scan.angle_max = 2 * math.pi #scan.angle_min = -math.pi #scan.angle_max = math.pi scan.angle_increment = 6.28 / self.num_readings #scan.angle_increment = 1 * (math.pi / 180) scan.time_increment = 1 / self.laser_frequency / self.num_readings scan.range_min = 0.2 scan.range_max = 6. self.ldata = list(array.array('f',self.rdata)) print("length of ldata :",len(self.ldata)) #print(self.ldata) ''' for i in range(self.num_readings-1,0,-1): scan.ranges.append(self.ldata[i]/100.) ''' for i in reversed(range(self.num_readings)): scan.ranges.append(self.ldata[i]/100.) self.pub_data(scan) except Exception as ex: print("Exception in rec_data", ex) self.disconnect()
def main(): # Create a VL53L0X object tof = VL53L0X.VL53L0X() # Start ranging tof.start_ranging(VL53L0X.VL53L0X_BETTER_ACCURACY_MODE) #timing = tof.get_timing() #if (timing < 20000): # timing = 20000 #rospy.loginfo("Timing %d ms" % (timing/1000)) pub = rospy.Publisher('lider_scan', LaserScan, queue_size=10) laser_scan = LaserScan() laser_scan.header.seq = 0 laser_scan.header.stamp = rospy.get_rostime() laser_scan.header.frame_id = "/world" laser_scan.angle_min = 0.0 laser_scan.angle_max = math.pi laser_scan.angle_increment = math.pi / 180 laser_scan.time_increment = 1.0 / 180 laser_scan.scan_time = laser_scan.time_increment * 360 laser_scan.range_min = 0.1 laser_scan.range_max = 2.0 r = rospy.Rate(90) angle = 0 while not rospy.is_shutdown(): laser_scan.ranges.append(float(tof.get_distance()) / 1000) if (angle >= 179): rospy.loginfo("pub") laser_scan.header.seq += 1 laser_scan.header.stamp = rospy.get_rostime() pub.publish(laser_scan) laser_scan.ranges = [] angle = 0 angle += 1 r.sleep() tof.stop_ranging()
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 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 _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 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 callback(self, data): """ Receive joystick data, formulate Twist message. """ horizscan = LaserScan() downscan = LaserScan() #Set up the 2D array to receive the incoming data image_array = [[0.0]*480]*640 #Convert the 1D array of bytes into the 2D array of floats for i in range(640 * 480): j = i * 4 depth_float = struct.unpack("f", data.data[j:j+4])[0] x_index = i % 640 y_index = i / 640 print float(depth_float) image_array[x_index][y_index] = float(depth_float) #Create the approximation of a horizontal planar LIDAR scan: #averaging the middle 10 pixel-depths across the 640-pixel wide image horiz_scan = [0.0]*640 for i in range(640): average_depth = 0.0 for j in range(235, 245): average_depth = average_depth + image_array[i][j] average_depth = average_depth / 10.0 horiz_scan[i] = average_depth #Create the approximation of a downwards planar LIDAR scan: #averaging the bottom 10 pixel-depths across the 640-pixel wide image down_scan = [0.0]*640 for i in range(640): average_depth = 0.0 for j in range(470, 480): average_depth = average_depth + image_array[i][j] average_depth = average_depth / 10.0 down_scan[i] = average_depth #Convert these 640-element scans into 64 blocks, each approximately 1 degree of the field of view horiz_blocks = [0.0]*64 for i in range(64): block_average = 0.0 for j in range((i * 10), (i * 10) + 10): block_average = block_average + horiz_scan[j] horiz_blocks[i] = block_average / 10.0 down_blocks = [0.0]*64 for i in range(64): block_average = 0.0 for j in range((i * 10), (i * 10) + 10): block_average = block_average + horiz_scan[j] down_blocks[i] = block_average / 10.0 #Convert the 64-element scans into "LaserScan" messages horizscan.angle_min = (57.0/2.0) / (Math.Pi * 180.0); horizscan.angle_max = -(57.0/2.0) / (Math.Pi * 180.0); downscan.angle_min = (57.0/2.0) / (Math.Pi * 180.0); downscan.angle_max = -(57.0/2.0) / (Math.Pi * 180.0); horizscan.angle_increment = (57.0/64.0) / (Math.Pi * 180.0); horizscan.time_increment = 0.0; downscan.angle_increment = (57.0/64.0) / (Math.Pi * 180.0); downscan.time_increment = 0.0; horizscan.range_min = 1.0; horizscan.range_max = 5.0; downscan.range_min = 1.0; downscan.range_max = 5.0; horizscan.ranges = horiz_blocks downscan.ranges = down_blocks #Set up the LaserScan messages to be published self.horizscan = horizscan self.downscan = downscan
import rospy from cwru_base.msg import Sonar from sensor_msgs.msg import LaserScan scan=LaserScan() def handle_sonar(msg, scan_pub): scan.header.frame_id = msg.header.frame_id scan.header.stamp = msg.header.stamp scan.ranges = [msg.dist for i in range(0,30)] scan_pub.publish(scan) if __name__ == '__main__': rospy.init_node('sonar_translator') scan_pub = rospy.Publisher('sonar_scan', LaserScan) rospy.Subscriber('sonar', Sonar, handle_sonar, scan_pub) scan.angle_min = rospy.get_param("~angle_min", -0.19) scan.angle_max = rospy.get_param("~angle_max", 0.19) scan.angle_increment = rospy.get_param("~angle_increment", 0.01266666667) scan.time_increment = rospy.get_param("~time_increment", 0.0) scan.scan_time = rospy.get_param("~scan_time", 0.05) scan.range_min = rospy.get_param("~range_min", 0.10) scan.range_max = rospy.get_param("~range_max", 2.0) rospy.spin()
def spin(self): encoders = [0,0] self.x = 0 # position in xy plane self.y = 0 self.th = 0 # NEED to reorder the laser scans and flip the laser around... this will not be intuitive for students!! # things that don't ever change scan_link = rospy.get_param('~frame_id',ROBOT_NAME+'base_laser_link') scan = LaserScan(header=rospy.Header(frame_id=ROBOT_NAME+scan_link)) scan.angle_min = -pi scan.angle_max = pi scan.angle_increment = pi/180.0 scan.range_min = 0.020 scan.range_max = 5.0 scan.time_increment = 1.0/(5*360) scan.scan_time = 0.2 odom = Odometry(header=rospy.Header(frame_id=ROBOT_NAME+"odom"), child_frame_id=ROBOT_NAME+'base_link') # main loop of driver r = rospy.Rate(5) iter_count = 0 rospy.sleep(4) #self.robot.requestScan() scan.header.stamp = rospy.Time.now() last_motor_time = rospy.Time.now() total_dth = 0.0 while not rospy.is_shutdown(): #print "spinning" iter_count += 1 if self.cmd_to_send != None: self.robot.send_command(self.cmd_to_send) self.cmd_to_send = None t_start = time.time() self.robot.requestScan() #time.sleep(.01) delta_t = (rospy.Time.now() - scan.header.stamp).to_sec() if delta_t-0.2 > 0.1: print "Iteration took longer than expected (should be 0.2) ", delta_t scan.header.stamp = rospy.Time.now() (scan.ranges, scan.intensities) = self.robot.getScanRanges() # repeat last measurement to simulate -pi to pi (instead of -pi to pi - pi/180) scan.ranges.append(scan.ranges[0]) scan.intensities.append(scan.intensities[0]) #print 'Got scan ranges %f' % (time.time() - t_start) # get motor encoder values curr_motor_time = rospy.Time.now() t_start = time.time() try: start_t = rospy.Time.now() left, right, left_speed, right_speed = self.robot.getMotors() delta_t = (rospy.Time.now() - scan.header.stamp).to_sec() # now update position information # might consider moving curr_motor_time down dt = (curr_motor_time - last_motor_time).to_sec() last_motor_time = curr_motor_time d_left = (left - encoders[0])/1000.0 d_right = (right - encoders[1])/1000.0 # might consider using speed instead! #print d_left, d_right, left_speed*dt/1000.0, right_speed*dt/1000.0 #d_left, d_right = left_speed*dt/1000.0, right_speed*dt/1000.0 encoders = [left, right] dx = (d_left+d_right)/2 dth = (d_right-d_left)/(BASE_WIDTH/1000.0) total_dth += dth x = cos(dth)*dx y = -sin(dth)*dx self.x += cos(self.th)*x - sin(self.th)*y self.y += sin(self.th)*x + cos(self.th)*y self.th += dth # prepare tf from base_link to odom quaternion = Quaternion() quaternion.z = sin(self.th/2.0) quaternion.w = cos(self.th/2.0) # prepare odometry odom.header.stamp = curr_motor_time odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = dx/dt odom.twist.twist.angular.z = dth/dt self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), curr_motor_time, ROBOT_NAME+"base_link", ROBOT_NAME+"odom" ) self.odomPub.publish(odom) #print 'Got motors %f' % (time.time() - t_start) except Exception as err: print "my error is " + str(err) t_start = time.time() self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1]))) try: bump_sensors = self.robot.getDigitalSensors() self.bumpPub.publish(Bump(leftFront=bump_sensors[0],leftSide=bump_sensors[1],rightFront=bump_sensors[2],rightSide=bump_sensors[3])) except: print "failed to get bump sensors!" # # send updated movement commands #print 'Set motors %f' % (time.time() - t_start) # publish everything #print "publishing scan!" self.scanPub.publish(scan) # wait, then do it again r.sleep()
#/odom publisher odomPub = rospy.Publisher('roombaOdom',Odometry) odoMsg = Odometry() odoMsg.header.frame_id = '/odom' odoMsg.child_frame_id = '/odom' odoMsg.pose.covariance = [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0] #/scan publisher scanPub = rospy.Publisher('scan',LaserScan) scanMsg = LaserScan() scanMsg.header.frame_id = 'ir_base' scanMsg.angle_min = 0.01 scanMsg.angle_max = 0.01 scanMsg.angle_increment = 0.02 scanMsg.time_increment = 0.0 scanMsg.scan_time = 0.0 scanMsg.range_min = 0.02 scanMsg.range_max = 1.5 scanMsg.intensities = [] #robot joint state (joint angles in radians) left_wheel_angle = 0.0; right_wheel_angle = 0.0; # Open a file logFile = open("log.txt", "r") # robot states w.r.t inertial frame
#!/usr/bin/python import rospy import sys from sensor_msgs.msg import LaserScan rospy.init_node('laserz') pub = rospy.Publisher('/base_scan', LaserScan) scan = LaserScan() scan.header.frame_id = '/base_laser_link' scan.angle_min = -2.26892805099 scan.angle_max = 2.26456475258 scan.angle_increment = 0.00436332309619 scan.time_increment = 1.73611115315e-05 scan.scan_time = 0.0500000007451 scan.range_min = 0.0230000000447 scan.range_max = 60.0 rate = rospy.Rate(20) while not rospy.is_shutdown(): rate.sleep() scan.header.stamp = rospy.Time.now() if '-x' in sys.argv: scan.ranges = [1.0]*200 if int(scan.header.stamp.secs) % 10 < 5: scan.ranges += [float('inf')] * 640 else: scan.ranges += [1.5] * 640 scan.ranges += [1.0]*200 else: scan.ranges= [float('inf')] * 1040
def _create_sensor_scan(self, sensor_input, sensor_config): # Joint State for Turtlebot stack # Note without this transform publisher the wheels will be white, stuck at 0, 0, 0 and RVIZ # will tell you that there is no transform from the wheel_links to the base. However, instead # of publishing a stream of pointless transforms, I have made the joint static in the URDF like this: # create.urdf.xacro: # <joint name="right_wheel_joint" type="fixed"> # NOTE This may prevent Gazebo from working with this model # Here is the old Joint State in case you want to restore it: # js = JointState(name=["left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint"], # position=[0, 0, 0, 0], velocity=[0, 0, 0, 0], effort=[0, 0, 0, 0]) # js.header.stamp = ros_now # self.joint_states_pub.publish(js) # Fake laser from "PING" Ultrasonic Sensor and IR Distance Sensor input: http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF # Use: roslaunch arlobot_rviz_launchers view_robot.launch to view this well for debugging and testing. # The purpose of this is two fold: # 1. It REALLY helps adjusting values in the Propeller and ROS when I can visualize the sensor output in RVIZ! # For this purpose, a lot of the parameters are a matter of personal taste. Whatever makes it easiest to visualize is best. # 2. I want to allow AMCL to use this data to avoid obstacles that the Kinect/Xtion miss. # For the second purpose, some of the parameters here may need to be tweaked, to adjust how large an object looks to AMCL. # Note that we should also adjust the distance at which AMCL takes this data into account either here or elsewhere. # Transform: http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29 # We do not need to broadcast a transform, because it is static and contained within the URDF files now. # Here is the static transform for reference anyway: # self._SonarTransformBroadcaster.sendTransform( # (0.1, 0.0, 0.2), # (0, 0, 0, 1), # ros_now, # "sonar_laser", # "base_link" # ) # Some help: # http://goo.gl/ZU9XrJ # TODO: I'm doing this all in degrees and then converting to Radians later. # Is there any way to do this in Radians? # I just don't know how to create and fill an array with "Radians" since they are not rational numbers, but multiples of PI, thus the degrees. num_readings = 360 # How about 1 per degree? laser_frequency = 100 # I'm not sure how to decide what to use here. # This is the fake distance to set all empty slots, and slots we consider "out of range" artificial_far_distance = 10 # Fill array with artificial_far_distance (not 0) and then overlap with real readings ranges = [artificial_far_distance] * num_readings # New idea here: # First, I do not think that this can be used for reliable for map generation. # If your room has objects that the Kinect cannot map, then you will probably need to modify # the room (cover mirrors, etc.) or try other laser scanner options. # So, since we only want to use it for cost planning, we should modify the data, because # it is easy for it to get bogged down with a lot of "stuff" everywhere. # From: # http://answers.ros.org/question/11446/costmaps-obstacle-does-not-clear-properly-under-sparse-environment/ # "When clearing obstacles, costmap_2d only trusts laser scans returning a definite range. Indoors, that makes # sense. Outdoors, most scans return max range, which does not clear intervening obstacles. A fake scan with # slightly shorter ranges can be constructed that does clear them out." # So, we need to set all "hits" above the distance we want to pay attention to to a distance very far away, # but just within the range_max (which we can set to anything we want), otherwise costmap will not clear items! # Also, 0 does not clear anything! So if we rotate, then it gets 0 at that point, and ignores it, so we need to # fill the unused slots with long distances. # NOTE: This does cause a "circle" to be drawn around the robot at the "artificalFarDistance", but it shouldn't # be a problem because we set artificial_far_distance to a distance greater than the planner uses. # So while it clears things, it shouldn't cause a problem, and the Kinect should override it for things # in between. # Use: # roslaunch arlobot_rviz_launchers view_robot.launch to view this well for debugging and testing. # max_range_accepted Testing: # TODO: More tweaking here could be done. # I think it is a trade-off, so there is no end to the adjustment that could be done. I did a lot of testing with gmapping while building a map. # Obviously this would be slightly different from using a map we do not update. It seems there are so many variables here that testing is difficult. # We could find one number works great in one situation but is hopeless in another. Having a comprehensive test course to test in multiple modes # for every possible value would be great, but I think it would take months! :) # REMEMBER, the costmap only pays attention out to a certain set for obstacle_range in costmap_common_params.yaml anyway. # Here are my notes for different values of "max_range_accepted": # 1 - looks good, and works ok, but I am afraid that the costmap gets confused with things popping in and out of sight all of the time, # causing undue wandering. # 2 - This producing less wandering due to things popping in and out of the field of view, BUT it also shows that we get odd affects at longer distances. i.e. # A doorframe almost always has a hit right in the middle of it. # In a hallway there is often a hit in the middle about 1.5 meters out. # .5 - This works very well to have the PING data ONLY provide obstacle avoidance, and immediately forget about said obstacles. # This prevents the navigation stack from fighting with the Activity Board code's built in safety stops, and instead navigate around obstacles before # the Activity Board code gets involved (other than to set speed reductions). # The only down side is if you tell ArloBot to go somewhere that he cannot due to low obstacles, he will try forever. He won't just bounce off of the obstacle, # but he will keep trying it and then go away, turn around, and try again over and over. He may even start wandering around the facility trying to find another way in, # but he will eventually come back and try it again. I'm not sure what the solution to this is though, because avoiding low lying obstacles and adding low lying # features to the map are really two different things. I think if this is well tuned to avoid low lying obstacles it probably will not work well for mapping features. # IF we could map features with the PING sensors, we wouldn't need the 3D sensor. :) # TODO: One option may be more PING sensors around back. Right now when the robot spins, it clears the obstacles behind it, because there are fewer sensors on the back side. # If the obstacle was seen all of the way around the robot, in the same spot, it may stop returning to the same location as soon as it turns around? # NOTE: The bump sensors on Turtlebot mark but do not clear. I'm not sure how that works out. It seems like every bump would end up being a "blot" in the landscape never to # be returned to, but maybe there is something I am missing? # NOTE: Could this be different for PING vs. IR? # Currently I'm not using IR! Just PING. The IR is not being used by costmap. It is here for seeing in RVIZ, and the Propeller board uses it for emergency stopping, # but costmap isn't watching it at the moment. I think it is too erratic for that. Convert cm to meters and add offset sensors = [for x in sensor_input if (x/100.0) + sensor_config["offset"] > sensor_config["threshold"] * 100 else (x/100.0) + sensor_config["offset"]] # Single Point code: for offset, position in dict["front"]: ranges[position] = sensors[offset] for pos in dict["rear"]: ranges[position] = sensors[offset] # LaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html scan = LaserScan() scan.header.stamp = ros_now scan.header.frame_id = sensor_config["frame id"] # For example: # scan.angle_min = -45 * M_PI / 180; // -45 degree # scan.angle_max = 45 * M_PI / 180; // 45 degree # if you want to receive a full 360 degrees scan, you should try setting min_angle to -pi/2 and max_angle to 3/2 * pi. # Radians: http://en.wikipedia.org/wiki/Radian#Advantages_of_measuring_in_radians scan.angle_min = 0 # Make sure the part you divide by num_readings is the same as your angle_max! # Might even make sense to use a variable here? scan.angle_increment = (2 * pi) / len(sensor_input) scan.time_increment = (1 / laser_frequency) / len(sensor_input) scan.range_min = sensor_config["range min"] scan.range_max = sensor_config["range max"] # This has to be above our "artificial_far_distance", otherwise "hits" at artificial_far_distance will be ignored, # which means they will not be used to clear the cost map! # in Meters Distances above this will be ignored scan.range_max = artificial_far_distance + 1 scan.ranges = ranges return scan
continue if tokens[0] == 'FLASER': msg = LaserScan() num_scans = int(tokens[1]) if num_scans != 180 or len(tokens) < num_scans + 9: rospy.logwarn("unsupported scan format") continue msg.header.frame_id = 'laser' t = rospy.Time(float(tokens[(num_scans + 8)])) msg.header.stamp = t msg.angle_min = -90.0 / 180.0 * pi msg.angle_max = 90.0 / 180.0 * pi msg.angle_increment = pi / (num_scans - 1.0) msg.time_increment = 0.2 / 360.0 msg.scan_time = 0.2 msg.range_min = 0.001 msg.range_max = 50.0 msg.ranges = [float(r) for r in tokens[2:(num_scans + 2)]] bag.write('scan', msg, t) #odom_x, odom_y, odom_theta = [float(r) for r in tokens[(num_scans + 2):(num_scans + 5)]] #tf_msg = make_tf_msg(odom_x, odom_y, odom_theta, t) #bag.write('tf', tf_msg, t) #odom_msg = mak_odom_msg(odom_x, odom_y, odom_theta,t) #bag.write("odom", odom_msg, t) elif tokens[0] == 'ODOM': odom_x, odom_y, odom_theta = [float(t) for t in tokens[1:4]]
def doAScan(direction): ######################################################## global moving scan_rate = 50 # hertz between steps angle_slack = 0.25 if direction: angle_start = 180 angle_stop = 0 angle_step = -1 else: angle_start = 0 angle_stop = 180 angle_step = 1 servo_pub.publish( angle_start ) moving = False scan = LaserScan() num_readings = 0 scan_time = rospy.Time.now() r = rospy.Rate(scan_rate) ranges = [] for angle in range(angle_start, angle_stop, angle_step): if moving: break servo_pub.publish( angle ) wait_start = rospy.Time.now() r.sleep() # while latest_std > std_threshold: # if rospy.Time.now() - wait_start > rospy.Duration(std_timeout): # rospy.loginfo("-W- range_to_laser timed out waiting for std_dev (%0.2f) to get below threshold (%0.2f)" % (latest_std, std_threshold) ) # break # rospy.loginfo("angle %d range:%0.2f" % (angle, latest_range)) num_readings += 1 ranges_ary = array(ranges[-4:]) # if ranges_ary.std() < 0.04: # scan.intensities.append(100) ranges.append(latest_range * scale) # else: # ranges.append(1e4) # scan.intensities.append(0) # if latest_range > 0: # scan.intensities.append( 1 / ( ranges_ary.std() * 10 + .1 ) ) if latest_range > 0: scan.intensities.append(1/latest_range) else: scan.intensities.append(0) scan.angle_min = -1.57 scan.angle_max = 1.57 if not direction: ranges.reverse() scan.intensities.reverse() scan.angle_min += angle_slack scan.angle_max += angle_slack scan.ranges = ranges scan.header.stamp = scan_time; scan.header.frame_id = 'scanner' # TODO: change this to 'laser_frame'. (create the transform first) scan.angle_increment = 3.14 / num_readings scan.time_increment = (1 / scan_rate) scan.range_min = 0.01 * scale scan.range_max = 2.0 * scale if not moving: scan_pub.publish(scan)
def _BroadcastOdometryInfo(self, lineParts): # This broadcasts ALL info from the Propeller based robot every time data comes in partsCount = len(lineParts) #rospy.logwarn(partsCount) if (partsCount < 8): # Just discard short lines, increment this as lines get longer pass try: x = float(lineParts[1]) y = float(lineParts[2]) # 3 is odom based heading and 4 is gyro based # If there is some way to "integrate" these, go for it! theta = float(lineParts[4]) vx = float(lineParts[5]) omega = float(lineParts[6]) #quaternion = tf.transformations.quaternion_from_euler(0, 0, theta) quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(theta / 2.0) quaternion.w = cos(theta / 2.0) rosNow = rospy.Time.now() # First, we'll publish the transform from frame odom to frame base_link over tf # Note that sendTransform requires that 'to' is passed in before 'from' while # the TransformListener' lookupTransform function expects 'from' first followed by 'to'. #This transform conflicts with transforms built into the Turtle stack # http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29 # This is done in/with the robot_pose_ekf because it can integrate IMU/gyro data # using an "extended Kalman filter" # REMOVE this "line" if you use robot_pose_ekf self._OdometryTransformBroadcaster.sendTransform( (x, y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rosNow, "base_footprint", "odom" ) # next, we'll publish the odometry message over ROS odometry = Odometry() odometry.header.frame_id = "odom" odometry.header.stamp = rosNow odometry.pose.pose.position.x = x odometry.pose.pose.position.y = y odometry.pose.pose.position.z = 0 odometry.pose.pose.orientation = quaternion odometry.child_frame_id = "base_link" odometry.twist.twist.linear.x = vx odometry.twist.twist.linear.y = 0 odometry.twist.twist.angular.z = omega #for Turtlebot stack from turtlebot_node.py # robot_pose_ekf needs these covariances and we may need to adjust them? # From: ~/turtlebot/src/turtlebot_create/create_node/src/create_node/covariances.py # This is not needed if not using robot_pose_ekf ''' odometry.pose.covariance = [1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e3] odometry.twist.covariance = [1e-3, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e3] ''' self._OdometryPublisher.publish(odometry) #"IMU" data from Gyro ''' # Based on code in TurtleBot source: # ~/turtlebot/src/turtlebot_create/create_node/src/create_node/gyro.py # It may make more sense to compute some of this on the Propeller board, # but for now I'm just trying to follow the TurtleBot Create code as beast I can current_time = rosNow #dt = (current_time - last_time).to_sec() #past_orientation = self.orientation #self.imu_data.header.stamp = sensor_state.header.stamp #self.imu_data.angular_velocity.z = (float(sensor_state.user_analog_input)-self.cal_offset)/self.cal_offset*self.gyro_measurement_range*(math.pi/180.0)*self.gyro_scale_correction #sign change #self.imu_data.angular_velocity.z = -1.0*self.imu_data.angular_velocity.z #self.orientation += self.imu_data.angular_velocity.z * dt #print orientation #(self.imu_data.orientation.x, self.imu_data.orientation.y, self.imu_data.orientation.z, self.imu_data.orientation.w) = PyKDL.Rotation.RotZ(self.orientation).GetQuaternion() #self.imu_data = odom.twist.twist = Twist(Vector3(d/dt, 0, 0), Vector3(0, 0, angle/dt)) #self.imu_pub.publish(self.imu_data) #self.imu_data.header.stamp = sensor_state.header.stamp #self.imu_data.angular_velocity.z = (float(sensor_state.user_analog_input)/self.gyro_measurement_range*(math.pi/180.0)*self.gyro_scale_correction) #sign change #self.imu_data.angular_velocity.z = -1.0*self.imu_data.angular_velocity.z #raw_orientation = past_orientation + self.imu_data.angular_velocity.z * dt #print orientation #(self.imu_data.orientation.x, self.imu_data.orientation.y, self.imu_data.orientation.z, self.imu_data.orientation.w) = PyKDL.Rotation.RotZ(raw_orientation).GetQuaternion() #self.imu_pub_raw.publish(self.imu_data) last_time = current_time imu = Imu() imu.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] # From Turtlebot, probably wrong. # You CANNOT set the orientation_covariance to -1, 0, ..., else you get this error: #[ERROR] [1405831732.096853617]: Covariance specified for measurement on topic imu is zero # The TurtleBot Create builds this in Python, but I'm not sure if I can or want to build orientation # myself? Does the Gyro give this? #imu.orientation_covariance = [-1,0,0,0,0,0,0,0,0] # This should indicate no data for this matrix imu.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6] # From Turtlebot, probably wrong. #imu.angular_velocity_covariance = [-1,0,0,0,0,0,0,0,0] # This should indicate no data for this matrix imu.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0] # This should indicate no data for this matrix #imu.orientation_covariance = [999999 , 0 , 0, 0, 9999999, 0, 0, 0, 999999] #imu.angular_velocity_covariance = [9999, 0 , 0, 0 , 99999, 0, 0 , 0 , 0.02] #imu.linear_acceleration_covariance = [0.2 , 0 , 0, 0 , 0.2, 0, 0 , 0 , 0.2] imu.linear_acceleration.x = 0 imu.linear_acceleration.y = 0 imu.linear_acceleration.z = 0 imu.angular_velocity.x = float(lineParts[7]) / 57.2957795130824 imu.angular_velocity.y = float(lineParts[8]) / 57.2957795130824 imu.angular_velocity.z = float(lineParts[9]) / 57.2957795130824 imu.orientation.x = 0 imu.orientation.y = 0 imu.orientation.z = 0 imu.orientation.w = 1.0 #imu.header.stamp = rospy.Time.now() imu.header.stamp = rosNow imu.header.frame_id = "gyro_link" #imu.header.frame_id = 'base_link' self._ImuPublisher.publish(imu) ''' # Joint State for Turtlebot stack # Note without this transform publisher the wheels will # be white, stuck at 0, 0, 0 and RVIZ will tell you that # there is no transform from the wheel_links to the base_ ''' # Instead of publishing a stream of pointless transforms, # How about if I just make the joint static in the URDF? # create.urdf.xacro: # <joint name="right_wheel_joint" type="fixed"> # NOTE This may prevent Gazebo from working with this model js = JointState(name = ["left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint"], position=[0,0,0,0], velocity=[0,0,0,0], effort=[0,0,0,0]) js.header.stamp = rosNow self.joint_states_pub.publish(js) ''' # Fake laser from "PING" Ultrasonic Sensor input: # http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF # Transform: http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29 ''' # We don't need to broadcast a transform, as it is static and contained within the URDF files self._SonarTransformBroadcaster.sendTransform( (0.1, 0.0, 0.2), (0, 0, 0, 1), rosNow, "sonar_laser", "base_link" ) ''' # Some help: http://books.google.com/books?id=2ZL9AAAAQBAJ&pg=PT396&lpg=PT396&dq=fake+LaserScan+message&source=bl&ots=VJMfSYXApG&sig=s2YgiHTA3i1OjVyPxp2aAslkW_Y&hl=en&sa=X&ei=B_vDU-LkIoef8AHsooHICA&ved=0CG0Q6AEwCQ#v=onepage&q=fake%20LaserScan%20message&f=false num_readings = 360 # How about 1 per degree? laser_frequency = 100 # I'm not sure how to decide what to use here. #ranges = [1] * num_readings # Fill array with fake "1" readings for testing ranges = [0] * num_readings # Fill array with 0 and then overlap with real readings pingRange0 = int(lineParts[7]) / 100.0 ranges[0] = pingRange0 ranges[1] = pingRange0 ranges[2] = pingRange0 ranges[3] = pingRange0 ranges[4] = pingRange0 ranges[5] = pingRange0 ranges[359] = pingRange0 ranges[358] = pingRange0 ranges[357] = pingRange0 ranges[356] = pingRange0 ranges[355] = pingRange0 # Is there a more concise way to code that array fill? # LaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html sonar_scan = LaserScan() sonar_scan.header.stamp = rosNow sonar_scan.header.frame_id = "ping_sensor_array" # For example: #scan.angle_min = -45 * M_PI / 180; // -45 degree #scan.angle_max = 45 * M_PI / 180; // 45 degree # if you want to receive a full 360 degrees scan, you should try setting min_angle to -pi/2 and max_angle to 3/2 * pi. # Radians: http://en.wikipedia.org/wiki/Radian#Advantages_of_measuring_in_radians sonar_scan.angle_min = 0 sonar_scan.angle_max = 2 * 3.14159 # Full circle sonar_scan.scan_time = 1 # I think this is only really applied for 3D scanning # Make sure the part you divide by num_readings is the same as your angle_max! # Might even make sense to use a variable here? sonar_scan.angle_increment = (2 * 3.14) / num_readings sonar_scan.time_increment = (1 / laser_frequency) / (num_readings) # From: http://www.parallax.com/product/28015 # Range: approximately 1 inch to 10 feet (2 cm to 3 m) # This should be adjusted based on the imaginary distance between the actual laser # and the laser location in the URDF file. Or else the adjustment somewhere else? sonar_scan.range_min = 0.02 # in Meters Distances below this number will be ignored sonar_scan.range_max = 3 # in Meters Distances above this will be ignored sonar_scan.ranges = ranges # "intensity" is a value specific to each laser scanner model. # It can safely be ignored self._SonarPublisher.publish(sonar_scan) except: rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
def _broadcast_odometry_info(self, line_parts): """ Broadcast all data from propeller monitored sensors on the appropriate topics. """ # If we got this far, we can assume that the Propeller board is initialized and the motors should be on. # The _switch_motors() function will deal with the _SafeToOparete issue if self._motorsOn == 0: self._switch_motors(True) parts_count = len(line_parts) # rospy.logwarn(partsCount) if parts_count != 8: # Just discard short/long lines, increment this as lines get longer pass x = float(line_parts[1]) y = float(line_parts[2]) # 3 is odom based heading and 4 is gyro based theta = float(line_parts[3]) # On ArloBot odometry derived heading works best. alternate_theta = float(line_parts[4]) vx = float(line_parts[5]) omega = float(line_parts[6]) quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(theta / 2.0) quaternion.w = cos(theta / 2.0) ros_now = rospy.Time.now() # First, we'll publish the transform from frame odom to frame base_link over tf # Note that sendTransform requires that 'to' is passed in before 'from' while # the TransformListener' lookupTransform function expects 'from' first followed by 'to'. # This transform conflicts with transforms built into the Turtle stack # http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29 # This is done in/with the robot_pose_ekf because it can integrate IMU/gyro data # using an "extended Kalman filter" # REMOVE this "line" if you use robot_pose_ekf self._OdometryTransformBroadcaster.sendTransform( (x, y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), ros_now, "base_footprint", "odom" ) # next, we will publish the odometry message over ROS odometry = Odometry() odometry.header.frame_id = "odom" odometry.header.stamp = ros_now odometry.pose.pose.position.x = x odometry.pose.pose.position.y = y odometry.pose.pose.position.z = 0 odometry.pose.pose.orientation = quaternion odometry.child_frame_id = "base_link" odometry.twist.twist.linear.x = vx odometry.twist.twist.linear.y = 0 odometry.twist.twist.angular.z = omega # Save last X, Y and Heading for reuse if we have to reset: self.lastX = x self.lastY = y self.lastHeading = theta self.alternate_heading = alternate_theta # robot_pose_ekf needs these covariances and we may need to adjust them. # From: ~/turtlebot/src/turtlebot_create/create_node/src/create_node/covariances.py # However, this is not needed because we are not using robot_pose_ekf # odometry.pose.covariance = [1e-3, 0, 0, 0, 0, 0, # 0, 1e-3, 0, 0, 0, 0, # 0, 0, 1e6, 0, 0, 0, # 0, 0, 0, 1e6, 0, 0, # 0, 0, 0, 0, 1e6, 0, # 0, 0, 0, 0, 0, 1e3] # # odometry.twist.covariance = [1e-3, 0, 0, 0, 0, 0, # 0, 1e-3, 0, 0, 0, 0, # 0, 0, 1e6, 0, 0, 0, # 0, 0, 0, 1e6, 0, 0, # 0, 0, 0, 0, 1e6, 0, # 0, 0, 0, 0, 0, 1e3] self._OdometryPublisher.publish(odometry) # Joint State for Turtlebot stack # Note without this transform publisher the wheels will # be white, stuck at 0, 0, 0 and RVIZ will tell you that # there is no transform from the wheel_links to the base_ # However, instead of publishing a stream of pointless transforms, # I have made the joint static in the URDF like this: # create.urdf.xacro: # <joint name="right_wheel_joint" type="fixed"> # NOTE This may prevent Gazebo from working with this model # Here is the old Joint State in case you want to restore it: # js = JointState(name=["left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint"], # position=[0, 0, 0, 0], velocity=[0, 0, 0, 0], effort=[0, 0, 0, 0]) # js.header.stamp = ros_now # self.joint_states_pub.publish(js) # Fake laser from "PING" Ultrasonic Sensor and IR Distance Sensor input: # http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF # Use: # roslaunch arlobot_rviz_launchers view_robot.launch # to view this well for debugging and testing. # The purpose of this is two fold: # 1. It REALLY helps adjusting values in the Propeller and ROS # when I can visualize the sensor output in RVIZ! # For this purpose, a lot of the parameters are a matter of personal taste. # Whatever makes it easiest to visualize is best. # 2. I want to allow AMCL to use this data to avoid obstacles that the Kinect/Xtion miss. # For the second purpose, some of the parameters here may need to be tweaked, # to adjust how large an object looks to AMCL. # Note that we should also adjust the distance at which AMCL takes this data # into account either here or elsewhere. # Transform: http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29 # We do not need to broadcast a transform, # because it is static and contained within the URDF files now. # Here is the static transform for reference anyway: # self._SonarTransformBroadcaster.sendTransform( # (0.1, 0.0, 0.2), # (0, 0, 0, 1), # ros_now, # "sonar_laser", # "base_link" # ) # Some help: # http://goo.gl/ZU9XrJ # TODO: I'm doing this all in degrees and then converting to Radians later. # Is there any way to do this in Radians? # I just don't know how to create and fill an array with "Radians" # since they are not rational numbers, but multiples of PI, thus the degrees. num_readings = 360 # How about 1 per degree? #num_reeading_multiple = 2 # We have to track this so we know where to put the readings! #num_readings = 360 * num_reeading_multiple # TODO: I am getting "artifacts" in the global cost map where points fail to clear, # even though they clear from the local. Maybe we need a higher resolution? laser_frequency = 100 # I'm not sure how to decide what to use here. # This is the fake distance to set all empty slots, and slots we consider "out of range" artificial_far_distance = 10 #ranges = [1] * num_readings # Fill array with fake "1" readings for testing # Fill array with artificial_far_distance (not 0) and then overlap with real readings ping_ranges = [artificial_far_distance] * num_readings # If we use 0, then it won't clear the obstacles when we rotate away, # because costmap2d ignores 0's and Out of Range! # Fill array with artificial_far_distance (not 0) and then overlap with real readings ir_ranges = [artificial_far_distance] * num_readings # New idea here: # First, I do not think that this can be used for reliable for map generation. # If your room has objects that the Kinect # cannot map, then you will probably need to modify the room (cover mirrors, etc.) or try # other laser scanner options. # SO, since we only want to use it for cost planning, we should modify the data, because # it is easy for it to get bogged down with a lot of "stuff" everywhere. # From: # http://answers.ros.org/question/11446/costmaps-obstacle-does-not-clear-properly-under-sparse-environment/ # "When clearing obstacles, costmap_2d only trusts laser scans returning a definite range. # Indoors, that makes sense. Outdoors, most scans return max range, which does not clear # intervening obstacles. A fake scan with slightly shorter ranges can be constructed that # does clear them out." # SO, we need to set all "hits" above the distance we want to pay attention to to a distance very far away, # but just within the range_max (which we can set to anything we want), # otherwise costmap will not clear items! # Also, 0 does not clear anything! So if we rotate, then it gets 0 at that point, and ignores it, # so we need to fill the unused slots with long distances. # NOTE: This does cause a "circle" to be drawn around the robot at the "artificalFarDistance", # but it shouldn't be a problem because we set # artificial_far_distance to a distance greater than the planner uses. # So while it clears things, it shouldn't cause a problem, and the Kinect should override it for things # in between. # Use: # roslaunch arlobot_rviz_launchers view_robot.launch # to view this well for debugging and testing. # Note that sensor orientation is important here! # If you have a different number or aim them differently this will not work! # TODO: Tweak this value based on real measurements! # TODO: Use both IR and PING sensors? # The offset between the pretend sensor location in the URDF # and real location needs to be added to these values. This may need to be tweaked. sensor_offset = 0.22545 # This will be the max used range, anything beyond this is set to "artificial_far_distance" max_range_accepted = .5 # max_range_accepted Testing: # TODO: More tweaking here could be done. # I think it is a trade-off, so there is no end to the adjustment that could be done. # I did a lot of testing with gmappingn while building a map. # Obviously this would be slightly different from using a map we do not update. # It seems there are so many variables here that testing is difficult. # We could find one number works great in one situation but is hopeless in another. # Having a comprehensive test course to test in multiple modes for every possible value would be great, # but I think it would take months! :) # REMEMBER, the costmap only pays attention out to a certain set # for obstacle_range in costmap_common_params.yaml anyway. # Here are my notes for different values of "max_range_accepted": # 1 - looks good, and works ok, but # I am afraid that the costmap gets confused with things popping in and out of sight all of the time, # causing undue wandering. # 2 - This producing less wandering due to things popping in and out of the field of view, # BUT it also shows that we get odd affects at longer distances. i.e. # A doorframe almost always has a hit right in the middle of it. # In a hallway there is often a hit in the middle about 1.5 meters out. # .5 - This works very well to have the PING data ONLY provide obstacle avoidance, # and immediately forget about said obstacles. # This prevents the navigation stack from fighting with the Activity Board code's # built in safety stops, and instead navigate around obstacles before the Activity Board # code gets involved (other than to set speed reductions). # The only down side is if you tell ArloBot to go somewhere that he cannot due to low obstacles, # he will try forever. He won't just bounce off of the obstacle, # but he will keep trying it and then go away, turn around, # and try again over and over. He may even start wandering around # the facility trying to find another way in, # but he will eventually come back and try it again. # I'm not sure what the solution to this is though, # because avoiding low lying obstacles and adding low lying # features to the map are really two different things. # I think if this is well tuned to avoid low lying obstacles it # probably will not work well for mapping features. # IF we could map features with the PING sensors, we wouldn't need the 3D sensor. :) # TODO: One option may be more PING sensors around back. # Right now when the robot spins, it clears the obstacles behind it, # because there are fewer sensors on the back side. # If the obstacle was seen all of the way around the robot, in the same spot, # it may stop returning to the same location as soon as it turns around? # NOTE: The bump sensors on Turtlebot mark but do not clear. # I'm not sure how that works out. It seems like every bump would # end up being a "blot" in the landscape never to be returned to, # but maybe there is something I am missing? # NOTE: Could this be different for PING vs. IR? # Currently I'm not using IR! Just PING. The IR is not being used by costmap. # It is here for seeing in RVIZ, and the Propeller board uses it for emergency stopping, # but costmap isn't watching it at the moment. I think it is too erratic for that. # Convert cm to meters and add offset ping_range0 = (int(line_parts[7]) / 100.0) + sensor_offset # Set to "out of range" for distances over "max_range_accepted" to clear long range obstacles # and use this for near range only. if ping_range0 > max_range_accepted: # Be sure "ultrasonic_scan.range_max" is set higher than this or # costmap will ignore these and not clear the cost map! ping_range0 = artificial_far_distance ir_range0 = (int(line_parts[8]) / 100.0) + sensor_offset # Convert cm to meters and add offset ping_range1 = (int(line_parts[9]) / 100.0) + sensor_offset if ping_range1 > max_range_accepted: ping_range1 = artificial_far_distance ir_range1 = (int(line_parts[10]) / 100.0) + sensor_offset ping_range2 = (int(line_parts[11]) / 100.0) + sensor_offset # Center forward sensor. if ping_range2 > max_range_accepted: ping_range2 = artificial_far_distance ir_range2 = (int(line_parts[12]) / 100.0) + sensor_offset # Center forward sensor. ping_range3 = (int(line_parts[13]) / 100.0) + sensor_offset if ping_range3 > max_range_accepted: ping_range3 = artificial_far_distance ir_range3 = (int(line_parts[14]) / 100.0) + sensor_offset ping_range4 = (int(line_parts[15]) / 100.0) + sensor_offset if ping_range4 > max_range_accepted: ping_range4 = artificial_far_distance ir_range4 = (int(line_parts[16]) / 100.0) + sensor_offset # Rear sensor, note these numbers can change if you add more sensors! ping_range5 = (int(line_parts[17]) / 100.0) + sensor_offset if ping_range5 > max_range_accepted: ping_range5 = artificial_far_distance ir_range5 = (int(line_parts[18]) / 100.0) + sensor_offset # The sensors are 11cm from center to center at the front of the base plate. # The radius of the base plate is 22.545 cm # = 28 degree difference (http://ostermiller.org/calc/triangle.html) sensor_seperation = 28 # Spread code: NO LONGER USED # TODO: This could make sense to return to if used properly, # allowing obstacles to "fill" the space and smoothly move "around" # the robot as it rotates and objects move across the view of the PING # sensors, instead of "jumping" from one point to the next. # # "sensor_spread" is how wide we expand the sensor "point" in the fake laser scan. # # For the purpose of obstacle avoidance, I think this can actually be a single point, # # Since the costmap inflates these anyways. # # #One issue I am having is it seems that the "ray trace" to the maximum distance # #may not line up with near hits, so that the global cost map is not being cleared! # #Switching from a "spread" to a single point may fix this? # #Since the costmap inflates obstacles anyway, we shouldn't need the spead should we? # # #sensor_spread = 10 # This is how wide of an arc (in degrees) to paint for each "hit" # #sensor_spread = 2 # Testing. I think it has to be even numbers? # # #NOTE: # #This assumes that things get bigger as they are further away. This is true of the PING's area, # #and while it may or may not be true of the object the PING sees, we have no way of knowing if # #the object fills the ping's entire field of view or only a small part of it, a "hit" is a "hit". # #However for the IR sensor, the objects are points, that are the same size regardless of distance, # #so we are clearly inflating them here. # # for x in range(180 - sensor_spread / 2, 180 + sensor_spread / 2): # PINGranges[x] = ping_range5 # Rear Sensor # IRranges[x] = ir_range5 # Rear Sensor # # for x in range((360 - sensor_seperation * 2) - sensor_spread / 2, # (360 - sensor_seperation * 2) + sensor_spread / 2): # PINGranges[x] = ping_range4 # IRranges[x] = ir_range4 # # for x in range((360 - sensor_seperation) - sensor_spread / 2, # (360 - sensor_seperation) + sensor_spread / 2): # PINGranges[x] = ping_range3 # IRranges[x] = ir_range3 # # for x in range(360 - sensor_spread / 2, 360): # PINGranges[x] = ping_range2 # IRranges[x] = ir_range2 # # Crosses center line # for x in range(0, sensor_spread /2): # PINGranges[x] = ping_range2 # IRranges[x] = ir_range2 # # for x in range(sensor_seperation - sensor_spread / 2, sensor_seperation + sensor_spread / 2): # PINGranges[x] = ping_range1 # IRranges[x] = ir_range1 # # for x in range((sensor_seperation * 2) - sensor_spread / 2, (sensor_seperation * 2) + sensor_spread / 2): # PINGranges[x] = ping_range0 # IRranges[x] = ir_range0 # Single Point code: #for x in range(180 - sensor_spread / 2, 180 + sensor_spread / 2): ping_ranges[180] = ping_range5 # Rear Sensor ir_ranges[180] = ir_range5 # Rear Sensor # for x in range((360 - sensor_seperation * 2) - sensor_spread / 2, # (360 - sensor_seperation * 2) + sensor_spread / 2): ping_ranges[360 - sensor_seperation * 2] = ping_range4 ir_ranges[360 - sensor_seperation * 2] = ir_range4 # for x in range((360 - sensor_seperation) - sensor_spread / 2, # (360 - sensor_seperation) + sensor_spread / 2): ping_ranges[360 - sensor_seperation] = ping_range3 ir_ranges[360 - sensor_seperation] = ir_range3 #for x in range(360 - sensor_spread / 2, 360): #PINGranges[x] = ping_range2 #IRranges[x] = ir_range2 # Crosses center line #for x in range(0, sensor_spread /2): ping_ranges[0] = ping_range2 ir_ranges[0] = ir_range2 #for x in range(sensor_seperation - sensor_spread / 2, sensor_seperation + sensor_spread / 2): ping_ranges[sensor_seperation] = ping_range1 ir_ranges[sensor_seperation] = ir_range1 #for x in range((sensor_seperation * 2) - sensor_spread / 2, (sensor_seperation * 2) + sensor_spread / 2): ping_ranges[sensor_seperation * 2] = ping_range0 ir_ranges[sensor_seperation * 2] = ir_range0 # LaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html ultrasonic_scan = LaserScan() infrared_scan = LaserScan() ultrasonic_scan.header.stamp = ros_now infrared_scan.header.stamp = ros_now ultrasonic_scan.header.frame_id = "ping_sensor_array" infrared_scan.header.frame_id = "ir_sensor_array" # For example: #scan.angle_min = -45 * M_PI / 180; // -45 degree #scan.angle_max = 45 * M_PI / 180; // 45 degree # if you want to receive a full 360 degrees scan, # you should try setting min_angle to -pi/2 and max_angle to 3/2 * pi. # Radians: http://en.wikipedia.org/wiki/Radian#Advantages_of_measuring_in_radians ultrasonic_scan.angle_min = 0 infrared_scan.angle_min = 0 #ultrasonic_scan.angle_max = 2 * 3.14159 # Full circle # Letting it use default, which I think is the same. #infrared_scan.angle_max = 2 * 3.14159 # Full circle # Letting it use default, which I think is the same. #ultrasonic_scan.scan_time = 3 # I think this is only really applied for 3D scanning #infrared_scan.scan_time = 3 # I think this is only really applied for 3D scanning # Make sure the part you divide by num_readings is the same as your angle_max! # Might even make sense to use a variable here? ultrasonic_scan.angle_increment = (2 * 3.14) / num_readings infrared_scan.angle_increment = (2 * 3.14) / num_readings ultrasonic_scan.time_increment = (1 / laser_frequency) / num_readings infrared_scan.time_increment = (1 / laser_frequency) / num_readings # From: http://www.parallax.com/product/28015 # Range: approximately 1 inch to 10 feet (2 cm to 3 m) # This should be adjusted based on the imaginary distance between the actual laser # and the laser location in the URDF file. # in Meters Distances below this number will be ignored REMEMBER the offset! ultrasonic_scan.range_min = 0.02 # in Meters Distances below this number will be ignored REMEMBER the offset! infrared_scan.range_min = 0.02 # This has to be above our "artificial_far_distance", # otherwise "hits" at artificial_far_distance will be ignored, # which means they will not be used to clear the cost map! # in Meters Distances above this will be ignored ultrasonic_scan.range_max = artificial_far_distance + 1 # in Meters Distances above this will be ignored infrared_scan.range_max = artificial_far_distance + 1 ultrasonic_scan.ranges = ping_ranges infrared_scan.ranges = ir_ranges # "intensity" is a value specific to each laser scanner model. # It can safely be ignored self._UltraSonicPublisher.publish(ultrasonic_scan) self._InfraredPublisher.publish(infrared_scan)