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 __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 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)
