예제 #1
0
 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.")
예제 #2
0
 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)