def __init__(self,
cusum_window_size=10,
frame="base_link",
sensor_id="laser1",
threshold=1):
self.data_ = []
self.data_.append([0, 0, 0])
self.i = 0
self.msg = 0
self.window_size = cusum_window_size
self.frame = frame
self.threshold = threshold
self.weight = 1.0
self.is_disable = False
self.is_over_lapping_required = False
ChangeDetection.__init__(self, 721)
rospy.init_node("laser_fusion", anonymous=False)
sensor_number = rospy.get_param("~sensor_number", 0)
self.sensor_id = rospy.get_param("~sensor_id", sensor_id)
self.pub = rospy.Publisher('collisions_' + str(sensor_number),
sensorFusionMsg,
queue_size=10)
self.dyn_reconfigure_srv = Server(laserConfig,
self.dynamic_reconfigureCB)
rospy.Subscriber("/scan_unified", LaserScan, self.laserCB)
rospy.spin()
def __init__(self,
cusum_window_size=10,
frame="base_link",
sensor_id="labeler1",
threshold=1000,
CHUNK=1024):
self.data_ = np.zeros(6)
self.i = 0
self.msg = 0
self.window_size = cusum_window_size
self.frame = frame
self.threshold = threshold
self.weight = 1.0
self.is_disable = False
self.is_filtered_available = False
self.is_collision_expected = False
self.is_over_lapping_required = False
self.is_covariance_detector_enable = False
rospy.init_node("imu_labeler", anonymous=False)
ChangeDetection.__init__(self, 6)
sensor_number = rospy.get_param("~sensor_number", 0)
self.pub = rospy.Publisher('collision_label', Header, queue_size=1)
self.sensor_id = rospy.get_param("~sensor_id", sensor_id)
self.dyn_reconfigure_srv = Server(imuLabelerConfig,
self.dynamic_reconfigureCB)
rospy.loginfo("Imu Labeler Ready")
input_topic = rospy.get_param("~input_topic", "/imu")
rospy.Subscriber(input_topic, Imu, self.imuCB)
rospy.spin()
def __init__(self,
cusum_window_size=10,
frame="base_link",
sensor_id="accel1",
threshold=60):
self.data_ = []
self.data_.append([0, 0, 0])
self.msg = 0
self.i = 0
self.window_size = cusum_window_size
self.frame = frame
self.threshold = np.ones(3) * threshold
self.weight = 1.0
self.is_disable = False
self.is_filtered_available = False
self.is_over_lapping_required = False
self.is_collision_expected = False
ChangeDetection.__init__(self)
rospy.init_node("accelerometer_fusion", anonymous=False)
self.subscriber_ = rospy.Subscriber("accel", AccelStamped, self.accCB)
self.subscriber_ = rospy.Subscriber("filter", controllerFusionMsg,
self.filterCB)
sensor_number = rospy.get_param("~sensor_number", 0)
self.sensor_id = rospy.get_param("~sensor_id", sensor_id)
self.pub = rospy.Publisher('collisions_' + str(sensor_number),
sensorFusionMsg,
queue_size=10)
self.dyn_reconfigure_srv = Server(accelerometerConfig,
self.dynamic_reconfigureCB)
rospy.loginfo("Accelerometer Ready for Fusion")
rospy.spin()
def __init__(self, max_samples=500, pace=2, cusum_window_size=10):
self.data_ = []
self.step_ = []
self.i = 0
self.msg = 0
self.window_size = cusum_window_size
RealTimePlotter.__init__(self, max_samples, pace, True)
ChangeDetection.__init__(self, 3)
rospy.init_node("amcl_monitoring_cusum", anonymous=True)
rospy.Subscriber("/odom", Odometry, self.amclCB)
plt.show()
rospy.spin()
plt.close("all")
def __init__(self, max_samples=500, pace=2, cusum_window_size=15):
self.data_ = []
self.step_ = []
self.i = 0
self.msg = 0
self.window_size = cusum_window_size
RealTimePlotter.__init__(self, max_samples, pace, False)
ChangeDetection.__init__(self, 1)
rospy.init_node("laser_detection_ros_cusum", anonymous=True)
rospy.Subscriber("/scan_unified", LaserScan, self.laserCB)
plt.show()
rospy.spin()
plt.close("all")
def __init__(self, max_samples=500, pace=2, cusum_window_size=10):
self.data_ = []
self.data_.append([0, 0, 0])
self.i = 0
self.msg = 0
self.window_size = cusum_window_size
RealTimePlotter.__init__(self, max_samples, pace)
ChangeDetection.__init__(self, 10, 512)
GaussPlot.__init__(self)
rospy.init_node("laser_detection_ros_gaus_cusum", anonymous=True)
rospy.Subscriber("scan_front", LaserScan, self.laserCB)
plt.legend()
plt.show()
rospy.spin()
plt.close("all")
def __init__(self,
cusum_window_size=10,
frame="base_link",
sensor_id="microphone1",
threshold=1000,
CHUNK=1024):
self.data_ = []
self.data_.append([0, 0, 0])
self.i = 0
self.msg = 0
self.window_size = cusum_window_size
self.frame = frame
self.threshold = threshold
self.CHUNK = CHUNK
self.weight = 1.0
self.is_disable = False
rospy.init_node("microphone_fusion", anonymous=False)
ChangeDetection.__init__(self, 1)
self.audio = pyaudio.PyAudio()
for i in range(self.audio.get_device_count()):
dev = self.audio.get_device_info_by_index(i)
print(dev)
self.device_index = 0
self.device = self.audio.get_device_info_by_index(self.device_index)
self.stream = self.audio.open(format=pyaudio.paInt16,
channels=1,
input_device_index=self.device_index,
rate=(int)(
self.device["defaultSampleRate"]),
input=True,
frames_per_buffer=self.CHUNK)
self.stream.start_stream()
r = rospy.Rate(10)
sensor_number = rospy.get_param("~sensor_number", 0)
self.sensor_id = rospy.get_param("~sensor_id", sensor_id)
self.pub = rospy.Publisher('collisions_' + str(sensor_number),
sensorFusionMsg,
queue_size=10)
self.dyn_reconfigure_srv = Server(microphoneConfig,
self.dynamic_reconfigureCB)
while not rospy.is_shutdown():
self.run()
#r.sleep()
self.stream.stop_stream()
rospy.spin()
def __init__(self, max_samples = 500, pace = 2, cusum_window_size = 10 ):
self.data_ = []
self.data_.append([0,0,0])
self.i = 0
self.msg = 0
self.window_size = cusum_window_size
RealTimePlotter.__init__(self,max_samples,pace)
ChangeDetection.__init__(self,3)
GaussPlot.__init__(self )
rospy.init_node("accelerometer_gauss_cusum", anonymous=True)
rospy.Subscriber("accel", AccelStamped, self.accCB)
self.dyn_reconfigure_srv = Server(accelerometerGaussConfig, self.dynamic_reconfigureCB)
plt.legend()
plt.show()
rospy.spin()
plt.close("all")
def __init__(self, max_samples=500, pace=2, cusum_window_size=10):
self.data_ = []
self.data_.append([0, 0, 0, 0, 0, 0])
self.i = 0
self.msg = 0
self.window_size = cusum_window_size
RealTimePlotter.__init__(self, max_samples, pace)
ChangeDetection.__init__(self, 6)
GaussPlot.__init__(self)
rospy.init_node("imu_gauss", anonymous=False)
rospy.Subscriber("imu/data", Imu, self.imuCB)
self.dyn_reconfigure_srv = Server(imuGaussConfig,
self.dynamic_reconfigureCB)
plt.legend()
plt.show()
rospy.spin()
plt.close("all")
def __init__(self, max_samples = 500, pace = 10, cusum_window_size = 20 ):
self.data_ = []
self.data_.append([0,0,0,0,0,0])
self.step_ = []
self.step_.append(0)
self.i = 0
self.msg = 0
self.window_size = cusum_window_size
RealTimePlotter.__init__(self,max_samples,pace)
ChangeDetection.__init__(self,6)
rospy.init_node("imu_cusum", anonymous=True)
input_topic = rospy.get_param("~input_topic", '/imu/data')
rospy.Subscriber(input_topic, Imu, self.imuCB)
self.dyn_reconfigure_srv = Server(imuGaussConfig, self.dynamic_reconfigureCB)
plt.legend()
plt.show()
rospy.spin()
plt.close("all")
def __init__(self, threshold = 10 ):
self.data_ = []
self.data_.append([0,0,0])
self.step_ = []
self.step_.append(0)
self.threshold = threshold
self.controller_id = 'velocity_controller'
self.i = 0
self.msg = 0
self.current_data = Twist()
self.frame = 'test'
self.is_disable = False
ChangeDetection.__init__(self,3)
rospy.init_node("controller_cusum", anonymous=True)
self.openLoop_ = Twist()
self.closeLoop_ = Twist()
self.pub = rospy.Publisher('filter', controllerFusionMsg, queue_size=1)
self.dyn_reconfigure_srv = Server(filterConfig, self.dynamic_reconfigureCB)
rospy.Subscriber("/base/twist_mux/command_navigation", Twist, self.openLoopCB)
rospy.Subscriber("/base/odometry_controller/odometry", Odometry, self.closeLoopCB)
rospy.spin()
def __init__(self, max_samples = 500, pace = 2, cusum_window_size = 10 ):
self.data_ = []
self.data_.append([0,0,0])
self.step_ = []
self.step_.append(0)
self.i = 0
self.msg = 0
self.current_data = Twist()
self.window_size = cusum_window_size
RealTimePlotter.__init__(self,max_samples,pace)
ChangeDetection.__init__(self,3)
rospy.init_node("controller_cusum", anonymous=True)
self.openLoop_ = Twist()
self.closeLoop_ = Twist()
rospy.Subscriber("/base/twist_mux/command_navigation", Twist, self.openLoopCB)
rospy.Subscriber("/base/odometry_controller/odometry", Odometry, self.closeLoopCB)
self.header = 0
plt.legend()
plt.show()
rospy.spin()
plt.close("all")
def __init__(self,
cusum_window_size=10,
frame="base_link",
sensor_id="accel1",
threshold=60):
self.data_ = np.zeros(6)
self.i = 0
self.msg = 0
self.window_size = cusum_window_size
self.frame = frame
self.threshold = np.ones(3) * threshold
self.weight = 1.0
self.is_disable = False
self.is_filtered_available = False
self.is_collision_expected = False
self.is_over_lapping_required = False
self.is_covariance_detector_enable = False
self.old_angle = 0
ChangeDetection.__init__(self, 6, 10)
rospy.init_node("imu_fusion", anonymous=False)
sensor_number = rospy.get_param("~sensor_number", 0)
self.pub = rospy.Publisher('collisions_' + str(sensor_number),
sensorFusionMsg,
queue_size=10)
self.sensor_id = rospy.get_param("~sensor_id", sensor_id)
self.dyn_reconfigure_srv = Server(imuConfig,
self.dynamic_reconfigureCB)
rospy.loginfo("Imu Ready for Fusion")
input_topic = rospy.get_param("~input_topic", "/imu/data")
print(input_topic)
rospy.Subscriber(input_topic, Imu, self.imuCB)
self.subscriber_ = rospy.Subscriber("filter", controllerFusionMsg,
self.filterCB)
rospy.spin()
CHANNELS = 1
RATE = 44100
CHUNK = 1024 # 1024bytes of data red from a buffer
RECORD_SECONDS = 5
# instantiate PyAudio (1)
audio = pyaudio.PyAudio()
stream = audio.open(format=FORMAT,
channels=CHANNELS,
rate=RATE,
input=True,
frames_per_buffer=CHUNK)
stream.start_stream()
detector_ = ChangeDetection(1024)
fig = plt.figure()
s = fig.add_subplot(211)
s2 = fig.add_subplot(212)
j = 0
window_size = 10
threshold = 2000000
CumSum = []
rawData = []
step = []
for i in range(0, int(RATE / CHUNK * RECORD_SECONDS)):
data = stream.read(CHUNK)
