def __init__(self):
self.sensor_values = LightSensorValues()
self.pub = rospy.Publisher('read_bagfile',
LightSensorValues,
queue_size=100)
rospy.Subscriber('/lightsensors', LightSensorValues,
self.sensor_callback)
self.on = True
self.bag_open = False
def output_decision(self):
if not self.on:
if self.bag_open:
print("----------------------")
print("Stop.")
self.bag.close()
self.bag_open = False
return
else:
if not self.bag_open:
filename = 'sensor_values.bag'
#rosparam.set_param("/current_bag_file", filename)
self.bag = rosbag.Bag(filename, 'w')
self.bag_open = True
print("----------------------")
print("Start recoding.")
s1 = self.sensor_values
s2 = LightSensorValues()
s2.left_side = s1.left_side
s2.right_side = s1.right_side
s2.left_forward = s1.left_forward
s2.right_forward = s1.right_forward
self.pub.publish(self.sensor_values)
self.bag.write('/event', self.sensor_values)
def __init__(self):
self.sensor_values = LightSensorValues()
print("start")
#self._decision = rospy.Publisher('/cluster',LightSensorValues,queue_size=100)
rospy.Subscriber('/event', LightSensorValues, self.sensor_callback)
self.k = 6
self.D = 4
self.reset()
self.g_sigma = np.array([np.identity(self.D) for i in range(self.k)])
self.pai = [1.0 / self.k for i in range(self.k)]
self.on = True
self.bag_open = False
self.output_decision()
def __init__(self):
self.sensor_values = LightSensorValues()
print("start")
#self._decision = rospy.Publisher('/cluster',LightSensorValues,queue_size=100)
rospy.Subscriber('/event', LightSensorValues, self.sensor_callback)
self.k = 8
self.reset()
self.g_sigma = [[[0.1,0],[0,0.1]],
[[0.1,0],[0,0.1]],
[[0.1,0],[0,0.1]],
[[0.1,0],[0,0.1]],
[[0.1,0],[0,0.1]],
[[0.1,0],[0,0.1]],
[[0.1,0],[0,0.1]],
[[0.1,0],[0,0.1]]]
self.pai = [1.0/self.k for i in range(self.k)]
self.on = True
self.bag_open = False
self.output_decision()
def talker():
rospy.init_node('lightsensors')
pub = rospy.Publisher('lightsensors', LightSensorValues, queue_size=1)
r = rospy.Rate(10)
if not rospy.is_shutdown():
try:
# rospy.loginfo(sensor1.ranges[0] * 1000)
d = LightSensorValues()
d.right_forward = range2led(sensor1.ranges)
d.right_side = range2led(sensor2.ranges)
d.left_side = range2led(sensor3.ranges)
d.left_forward = range2led(sensor4.ranges)
pub.publish(d)
except:
rospy.logerr("Converting Sensor Data Failed")
r.sleep()
#!/usr/bin/env python
import sys, rospy
from raspimouse_ros.msg import LightSensorValues
if __name__ == '__main__':
devfile = '/dev/rtlightsensor0'
rospy.init_node('rtlightsensors')
pub = rospy.Publisher('lightsensors', LightSensorValues, queue_size=1)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
try:
with open(devfile, 'r') as f:
data = f.readline().split()
data = [int(e) for e in data]
d = LightSensorValues()
d.right_forward = int(data[0])
d.right_side = int(data[1])
d.left_side = int(data[2])
d.left_forward = int(data[3])
# d.sum_all = sum(data)
# d.sum_forward = data[0] + data[3]
pub.publish(d)
except:
rospy.logerr("cannot open " + devfile)
rate.sleep()