scripts/robo_explorer.py

   1 #!/usr/bin/env python
   2 import roslib; roslib.load_manifest('roboint')
   3 import rospy
   4 from math import *
   5 from sensor_msgs.msg import Range
   6 from roboint.msg import Inputs
   7
   8
   9 class RoboExplorer:
  10         def __init__(self):
  11                 rospy.init_node('robo_explorer')
  12
  13                 self.last_time = rospy.Time.now()
  14                 self.pub_sonar = rospy.Publisher("sonar", Range, queue_size=16)
  15                 rospy.Subscriber("ft/get_inputs", Inputs, self.inputsReceived)
  16
  17                 rospy.spin()
  18
  19         def inputsReceived(self, msg):
  20                 current_time = rospy.Time.now()
  21
  22                 #self.update_odometry(msg, current_time)
  23                 if (current_time - self.last_time).to_nsec() > 100e6: # send every 100ms
  24                         #self.send_odometry(msg, current_time)
  25                         self.send_range(msg, current_time)
  26                         self.last_time = current_time
  27
  28         def send_range(self, msg, current_time):
  29                 # ultra sonic range finder
  30                 scan = Range()
  31                 scan.header.stamp = current_time
  32                 scan.header.frame_id = "forward_sensor"
  33                 scan.radiation_type = 0
  34                 scan.field_of_view = 60*pi/180
  35                 scan.min_range = 0.0
  36                 scan.max_range = 4.0
  37                 scan.range = msg.d1/100.0
  38                 self.pub_sonar.publish(scan)
  39
  40 if __name__ == '__main__':
  41         RoboExplorer()