def scan_callback(self, msg):
angle = msg.angle_min
d_angle = msg.angle_increment
sum_x = 0
sum_y = 0
sum_xx = 0
sum_xy = 0
num = 0
for r in msg.ranges:
if angle > self.min_angle and angle < self.max_angle and r < msg.range_max:
x = math.sin(angle) * r
y = math.cos(angle) * r
sum_x += x
sum_y += y
sum_xx += x*x
sum_xy += x*y
num += 1
angle += d_angle
if num > 0:
denominator = num*sum_xx-sum_x*sum_x
if denominator != 0:
angle=math.atan2((-sum_x*sum_y+num*sum_xy)/(denominator), 1)
#print("Scan Angle: %s"%str(angle))
relay = ScanAngle()
relay.header = msg.header
relay.scan_angle = angle
with self.lock:
if self._laser_scan_angle_publisher:
self._laser_scan_angle_publisher.publish(relay)
else:
rospy.logerr("Please point me at a wall.")
def gyro_callback(self, msg):
with self.lock:
angle = quat_to_angle(msg.orientation)
self._gyro_angle = angle
self._gyro_time = msg.header.stamp
if self._initial_gyro_offset:
gyro_scan_angle = ScanAngle()
gyro_scan_angle.header = msg.header
gyro_scan_angle.scan_angle = angle - self._initial_gyro_offset
self._gyro_scan_angle_publisher.publish(gyro_scan_angle)
if self._running:
error_scan_angle = ScanAngle()
error_scan_angle.header = msg.header
error_scan_angle.scan_angle = math.fabs(angle - self._initial_gyro_offset - self._scan_angle)
#if error_scan_angle.scan_angle < self._epsilon: # don't spam with errors greater than what we're looking for
self._error_scan_angle_publisher.publish(error_scan_angle)