2 import roslib; roslib.load_manifest('roboint')
5 from sensor_msgs.msg import Range
6 from roboint.msg import Inputs
11 rospy.init_node('robo_explorer')
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)
19 def inputsReceived(self, msg):
20 current_time = rospy.Time.now()
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
28 def send_range(self, msg, current_time):
29 # ultra sonic range finder
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
37 scan.range = msg.d1/100.0
38 self.pub_sonar.publish(scan)
40 if __name__ == '__main__':