def filteredAccCB(self, msg):
self.addData(
[msg.accel.linear.x, msg.accel.linear.y, msg.accel.angular.z])
if (len(self.samples) > self.window_size):
self.samples.pop(0)
output_msg = sensorFusionMsg()
self.changeDetection(len(self.samples))
cur = np.array(self.cum_sum, dtype=object)
#Filling Message
output_msg.header.frame_id = self.frame
output_msg.window_size = self.window_size
#print ("Accelerations " , x,y,z)
if any(t > self.threshold for t in cur):
output_msg.msg = sensorFusionMsg.ERROR
print("Collision Filter")
output_msg.header.stamp = rospy.Time.now()
output_msg.sensor_id.data = self.sensor_id
output_msg.data = cur
output_msg.weight = self.weight
if not self.is_disable and self.is_collision_expected:
self.pub.publish(output_msg)
def gps_CB(self, msg):
X = np.array([[msg.latitude, msg.longitude, msg.altitude]])
fb_msg = sensorFusionMsg()
fb_msg.sensor_id.data = "GPS"
"""
if self.is_training:
self.clf.fit(X)
else:
detected_class = self.clf.predict(X)
if detected_class < 0:
fb_msg.msg = sensorFusionMsg.ERROR
rospy.logwarn('Event Detected')
self.pub.publish(fb_msg)
"""
self._ax[0].scatter(X.reshape(3, -1)[0, -1],
X.reshape(3, -1)[1, -1],
color=self.color[-1])
self._ax[1].scatter(X.reshape(3, -1)[0, -1],
X.reshape(3, -1)[2, -1],
color=self.color[-2])
self._ax[2].scatter(X.reshape(3, -1)[1, -1],
X.reshape(3, -1)[2, -1],
color=self.color[-3])
plt.draw()
#plt.show(False)
rospy.sleep(0.1)
def accCB(self, msg):
if self.is_over_lapping_required:
self.addData(
[msg.accel.linear.x, msg.accel.linear.y, msg.accel.angular.z])
if len(self.samples) > self.window_size:
self.samples.pop(0)
else:
if (self.i < self.window_size) and len(
self.samples) < self.window_size:
self.addData([
msg.accel.linear.x, msg.accel.linear.y, msg.accel.angular.z
])
self.i = self.i + 1
else:
self.samples.pop(0)
return
output_msg = sensorFusionMsg()
current_mean = np.mean(self.samples, axis=0)
#print (current_mean)
tmp = (np.array(self.last_mean) - np.array(current_mean))
x, y, z = 9.81 * (tmp / 255) * 2
magnitude = np.sqrt(np.power(x, 2) + np.power(y, 2) + np.power(z, 2))
#print ("magnitude ", magnitude)
#print (np.arccos(x/magnitude), np.arccos(y/magnitude), np.arccos(z/magnitude))
output_msg.angle = np.arctan2(
y, x) #np.arccos(x/magnitude)#np.arctan2(y,x)
#Detecting Collisions
self.changeDetection(len(self.samples))
cur = np.array(self.cum_sum, dtype=object)
#Filling Message
output_msg.header.frame_id = self.frame
output_msg.window_size = self.window_size
#print ("Accelerations " , x,y,z)
self.i = 0
output_msg.header.stamp = rospy.Time.now()
output_msg.sensor_id.data = self.sensor_id
output_msg.data = cur
output_msg.weight = self.weight
if any(cur[0:3] > self.threshold):
output_msg.msg = sensorFusionMsg.ERROR
print("Collision FOUND")
if self.is_collision_expected and self.is_filtered_available:
print("Colliison Filtered")
output_msg.msg = sensorFusionMsg.WARN
self.is_collision_expected = False
if not self.is_disable:
self.pub.publish(output_msg)
def laserCB(self, msg):
if self.is_over_lapping_required:
self.addData([i / msg.range_max for i in msg.ranges])
while len(self.samples) > self.window_size:
self.samples.pop(0)
else:
if (self.i < self.window_size
and len(self.samples) < self.window_size):
self.addData([i / msg.range_max for i in msg.ranges])
self.i = self.i + 1
else:
self.samples.pop(0)
return
msg = sensorFusionMsg()
self.i = 0
self.changeDetection(len(self.samples))
cur = np.array(self.cum_sum)
cur = np.nan_to_num(cur)
cur[np.isnan(cur)] = 0
cur = np.var(cur)
#Detecting Collisions
#if any(t > self.threshold for t in cur):
#if any(t > self.threshold for t in cur):
data = list()
if cur > 10000000:
data.append(0) #TODO
else:
data.append(cur)
if cur > self.threshold and not self.is_disable:
#Filling Message
msg.header.frame_id = self.frame
msg.window_size = self.window_size
msg.header.stamp = rospy.Time.now()
msg.msg = sensorFusionMsg.ERROR
msg.sensor_id.data = self.sensor_id
msg.data = data
msg.weight = self.weight
self.pub.publish(msg)
def imuCB(self,msg):
X = np.array([[msg.latitude, msg.longitude, msg.speed]])
fb_msg = sensorFusionMsg()
fb_msg.sensor_id.data = "GPS"
if self.is_training:
self.clf.fit(X)
else:
detected_class = self.clf.predict(X)
if detected_class < 0:
fb_msg.msg = sensorFusionMsg.ERROR
rospy.logwarn('Event Detected')
self.pub.publish(fb_msg)
def run(self):
try:
data = self.stream.read(self.CHUNK)
except IOError as ex:
if ex[1] != pyaudio.paInputOverflowed:
raise
rospy.logwarn("Sync error")
return
amplitude = np.fromstring(data, np.int16)
if self.i < self.window_size:
self.addData(amplitude)
self.i = self.i + 1
if len(self.samples) is self.window_size:
self.samples.pop(0)
return
msg = sensorFusionMsg()
self.i = 0
self.changeDetection(len(self.samples))
cur = np.array(self.cum_sum)
cur = np.nan_to_num(cur)
cur[np.isnan(cur)] = 0
#Filling Message
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame
msg.window_size = self.window_size
#Detecting Collisions
suma = np.sum(np.array(self.cum_sum, dtype=object))
var = np.var(np.array(self.cum_sum, dtype=object))
#if var > self.threshold:
print("S", suma)
print(var)
if suma > self.threshold:
#print ("COllision")
msg.sensor_id.data = self.sensor_id
msg.data = np.array(suma)
msg.weight = self.weight
msg.msg = sensorFusionMsg.ERROR
if not self.is_disable:
self.pub.publish(msg)
def base_state_CB(self, msg):
for i in range(4):
X = np.array(
[[msg.prop_speed[i], msg.prop_pos[i], msg.steer_pos[i]]])
fb_msg = sensorFusionMsg()
fb_msg.sensor_id.data = "wheel_" + str(i)
if self.is_training:
self.clf[i].fit(X)
else:
detected_class = self.clf[i].predict(X)
if detected_class < 0:
fb_msg.msg = sensorFusionMsg.ERROR
rospy.logwarn("Error " + str(i))
self.pub[i].publish(fb_msg)
def publishMsg(self,data):
output_msg = sensorFusionMsg()
#Filling Message
output_msg.header.frame_id = self.frame
output_msg.window_size = self.window_size
#print ("Accelerations " , x,y,z)
output_msg.sensor_id.data = self.sensor_id
#if any(t > self.threshold for t in data[3:5]):
print data
if any(t for t in data) > self.threshold:
rospy.logwarn("Collision")
output_msg.msg = sensorFusionMsg.ERROR
#data[0] = data[0] - 10.8 #TODO
#data[1] = data[1] - 6.67#TODO
#data[2] = data[2] + 14.02#TODO
output_msg.data = data
output_msg.header.stamp = rospy.Time.now()
if not self.is_disable:
self.pub.publish(output_msg)
def publishMsg(self, data):
output_msg = sensorFusionMsg()
#Filling Message
output_msg.header.frame_id = self.frame
output_msg.window_size = self.window_size
#print ("Accelerations " , x,y,z)
#print len(self.cum_sum)
suma = np.nansum(self.cum_sum)
var = np.var(self.cum_sum)
rospy.loginfo("Sum %d", suma)
rospy.loginfo("Var %f", var)
if suma >= self.threshold and not self.is_disable:
rospy.logwarn("Sum %d", suma)
rospy.logwarn("Var %f", var)
output_msg.msg = sensorFusionMsg.ERROR
output_msg.header.stamp = rospy.Time.now()
output_msg.sensor_id.data = self.sensor_id
tmp = list()
tmp.append(suma)
output_msg.data = tmp
output_msg.weight = self.weight
self.pub.publish(output_msg)
def imuCB(self, msg):
X = np.array([[
msg.linear_acceleration.x, msg.linear_acceleration.y,
msg.linear_acceleration.z
]])
if self.is_training:
self.clf.fit(X)
else:
fb_msg = sensorFusionMsg()
fb_msg.sensor_id.data = "sv_detector"
fb_msg.data = X.flatten()
detected_class = self.clf.predict(X)
if detected_class > 0:
rospy.logwarn('Event Detected')
fb_msg.msg = sensorFusionMsg.WARN
else:
fb_msg.msg = sensorFusionMsg.OK
self.event_publisher.publish(fb_msg)
def imuCB(self, msg):
if self.is_over_lapping_required:
self.addData([
msg.linear_acceleration.x,
msg.linear_acceleration.y,
msg.linear_acceleration.z, #]), #]) #Just Linear For Testing
msg.angular_velocity.x,
msg.angular_velocity.y,
msg.angular_velocity.z
]) #Angula
while len(self.samples) > self.window_size:
self.samples.pop(0)
else:
if (self.i < self.window_size) and len(
self.samples) < self.window_size:
self.addData([
msg.linear_acceleration.x,
msg.linear_acceleration.y,
msg.linear_acceleration.
z, #])#, #]) #Just Linear For Testing
msg.angular_velocity.x,
msg.angular_velocity.y,
msg.angular_velocity.z
]) #Angula
self.i = self.i + 1
if len(self.samples) is self.window_size:
self.samples.pop(0)
return
output_msg = sensorFusionMsg()
self.i = 0
self.changeDetection(len(self.samples))
cur = np.array(self.cum_sum, dtype=object)
#Filling Message
output_msg.header.frame_id = self.frame
output_msg.window_size = self.window_size
covariance = np.var(cur)
#current_angle = math.atan2(self.samples[-1][1] - self.samples[-2][1],self.samples[-2][0] - self.samples[-1][0])
#output_msg.angle = current_angle
output_msg.header.stamp = rospy.Time.now()
output_msg.sensor_id.data = self.sensor_id
output_msg.data = cur
output_msg.weight = self.weight
if self.is_covariance_detector_enable:
if any(covariance > self.threshold):
rospy.logwarn(covariance)
rospy.logwarn(cur > self.threshold)
output_msg.msg = sensorFusionMsg.ERROR
if self.is_collision_expected and self.is_filtered_available:
print("Colliison Filtered")
output_msg.msg = sensorFusionMsg.WARN
self.is_collision_expected = False
if not self.is_disable:
self.pub.publish(output_msg)
else:
print("here", cur[0:3])
if any(cur[0:3] > self.threshold):
print("here2")
rospy.logwarn(cur)
if cur[5] > self.threshold[2]:
rospy.logwarn("Angle")
x, y, z = (cur[0:3] > self.threshold)
if x and not y:
print("NORTH")
if not x and y:
print("EAST")
if x and y:
print("NE")
output_msg.msg = sensorFusionMsg.ERROR
if self.is_collision_expected and self.is_filtered_available:
print("Colliison Filtered")
output_msg.msg = sensorFusionMsg.WARN
self.is_collision_expected = False
if not self.is_disable:
self.pub.publish(output_msg)