def imu_talker():
# Create new phidget22 accelerometer object
accelerometer = Accelerometer()
# Check if imu can be found by OS
accelerometer.openWaitForAttachment(1000)
try:
pub = rospy.Publisher('crashed', Bool, queue_size=10)
rospy.init_node('crash_detector', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# Get current acceleration
acceleration = accelerometer.getAcceleration()
# Check if y acceleration has a large negative value in gs
if acceleration[1] < -0.2:
# In this case we have probably crashed
crashed = True
else:
# In this case we probably didn't crash
crashed = False
pub.publish(crashed)
rate.sleep()
except rospy.ROSInterruptException:
accelerometer.close()
class PhidgetKalman(object):
#Read the gyro and acceleromater values from MPU6050
def __init__(self):
self.kalmanX = KalmanAngle()
self.kalmanY = KalmanAngle()
self.RestrictPitch = True #Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
self.radToDeg = 57.2957786
self.kalAngleX = 0
self.kalAngleY = 0
try:
self.accel = Accelerometer() #ch.getAcceleration()
self.gyro = Gyroscope() #ch.getAngularRate()
except Exception as e:
print(
"Cannot initalize, try checking you have the Phidget22 library installed."
)
print("Error:", e)
sys.exit()
# return self.accel, self.gyro
def start(self):
try:
self.accel.openWaitForAttachment(1000)
self.gyro.openWaitForAttachment(1000)
except Exception as e:
print(
"Issue with attaching to IMU. The IMU maybe connected to something else right now.."
)
print("Error:", e)
sys.exit()
def stop(self):
self.accel.close()
self.gyro.close()
print("Stopping")
def get_angles(self, measure_time=5):
#Keep going
time.sleep(1)
accX, accY, accZ = self.accel.getAcceleration()
if (self.RestrictPitch):
roll = math.atan2(accY, accZ) * self.radToDeg
pitch = math.atan(-accX / math.sqrt((accY**2) +
(accZ**2))) * self.radToDeg
else:
roll = math.atan(accY / math.sqrt((accX**2) +
(accZ**2))) * self.radToDeg
pitch = math.atan2(-accX, accZ) * self.radToDeg
# print(roll)
self.kalmanX.setAngle(roll)
self.kalmanX.setAngle(pitch)
gyroXAngle = roll
gyroYAngle = pitch
compAngleX = roll
compAngleY = pitch
timer = time.time()
compare_timer = time.time()
flag = 0
try:
while int(time.time()) < int(measure_time) + int(
compare_timer
): #Should only run for about 5 seconds <- Could make lower
#Read Accelerometer raw value
accX, accY, accZ = self.accel.getAcceleration()
#Read Gyroscope raw value
gyroX, gyroY, gyroZ = self.gyro.getAngularRate()
dt = time.time() - timer
timer = time.time()
if (self.RestrictPitch):
roll = math.atan2(accY, accZ) * self.radToDeg
pitch = math.atan(
-accX / math.sqrt((accY**2) +
(accZ**2))) * self.radToDeg
else:
roll = math.atan(
accY / math.sqrt((accX**2) +
(accZ**2))) * self.radToDeg
pitch = math.atan2(-accX, accZ) * self.radToDeg
gyroXRate = gyroX / 131
gyroYRate = gyroY / 131
if (self.RestrictPitch):
if ((roll < -90 and self.kalAngleX > 90)
or (roll > 90 and self.kalAngleX < -90)):
self.kalmanX.setAngle(roll)
complAngleX = roll
self.kalAngleX = roll
gyroXAngle = roll
else:
self.kalAngleX = self.kalmanX.getAngle(
roll, gyroXRate, dt)
if (abs(self.kalAngleX) > 90):
gyroYRate = -gyroYRate
self.kalAngleY = self.kalmanX.getAngle(
pitch, gyroYRate, dt)
else:
if ((pitch < -90 and self.kalAngleY > 90)
or (pitch > 90 and self.kalAngleY < -90)):
self.kalmanX.setAngle(pitch)
complAngleY = pitch
self.kalAngleY = pitch
gyroYAngle = pitch
else:
self.kalAngleY = self.kalmanX.getAngle(
pitch, gyroYRate, dt)
if (abs(self.kalAngleY) > 90):
gyroXRate = -gyroXRate
self.kalAngleX = self.kalmanX.getAngle(
roll, gyroXRate, dt)
#angle = (rate of change of angle) * change in time
gyroXAngle = gyroXRate * dt
gyroYAngle = gyroYAngle * dt
#compAngle = constant * (old_compAngle + angle_obtained_from_gyro) + constant * angle_obtained from accelerometer
compAngleX = 0.93 * (compAngleX + gyroXRate * dt) + 0.07 * roll
compAngleY = 0.93 * (compAngleY +
gyroYRate * dt) + 0.07 * pitch
if ((gyroXAngle < -180) or (gyroXAngle > 180)):
gyroXAngle = self.kalAngleX
if ((gyroYAngle < -180) or (gyroYAngle > 180)):
gyroYAngle = self.kalAngleY
self.X_angle = self.kalAngleX
self.Y_angle = self.kalAngleY
#print("Angle X: " + str(self.kalAngleX)+" " +"Angle Y: " + str(self.kalAngleY))
#print(str(roll)+" "+str(gyroXAngle)+" "+str(compAngleX)+" "+str(self.kalAngleX)+" "+str(pitch)+" "+str(gyroYAngle)+" "+str(compAngleY)+" "+str(self.kalAngleY))
time.sleep(0.005)
except KeyboardInterrupt:
print("test")
return self.X_angle, self.Y_angle
class PlotCamLiteWindow_Monitor(QMainWindow):
def __init__(self):
super().__init__()
# init variables
self.depth_cam_proc = None
self.accelerometer = None
self.metadata = None
self.experiment_path = None
self.is_streaming = multiprocessing.Value('i', False)
self.current_plot_number = 0
self.x = 0.0
self.y = 0.0
self.init_ui()
self.title = "PlotCam Lite - %s and PyQt5" % PLATFORM
def init_ui(self):
"""
Initalizes the UI.
Connects methods to the buttons and begins the background processes.
"""
if not pcl_config["monitor"]:
self.setFixedSize(850, 630)
with disable_logging(logging.DEBUG):
loadUi(pcl_config["main_window_ui_path"], self)
# Set Window Title
self.setWindowTitle(PROGRAM_TITLE)
# Set Window Icon
self.setWindowIcon(QIcon(ICON_IMAGE_PATH))
# Add actions to menu bar
self.add_actions()
# Start the camera stream
self.start_stream()
# Start the accelerometer
self.start_accelerometer()
# Connect buttons to methods
self.take_picture_button.clicked.connect(self.take_picture)
self.exit_button.clicked.connect(self.close)
self.new_file_button.clicked.connect(self.new_experiment_dialog)
self.load_experiment_button.clicked.connect(self.browse_files)
# disable the take pic btn until an experiment is set
self.take_picture_button.setEnabled(False)
self.alert = QSound(ALERT_AUDIO_PATH)
def add_actions(self):
"""
Connects the menu bar actions to their respective functions.
"""
# File Menu
self.actionClose.triggered.connect(self.close)
# Edit Menu
self.action640x480.triggered.connect(lambda: self.set_res(480, 640))
self.action1280x720.triggered.connect(lambda: self.set_res(720, 1280))
self.action15.triggered.connect(lambda: self.set_fps(15))
self.action30.triggered.connect(lambda: self.set_fps(30))
self.actionMonitor.triggered.connect(lambda: self.set_view("view"))
self.actionVR.triggered.connect(lambda: self.set_fps(30))
self.resGroup = QActionGroup(self)
self.resGroup.addAction(self.action640x480)
self.resGroup.addAction(self.action1280x720)
self.resGroup.setExclusive(True)
self.action640x480.setCheckable(True)
self.action1280x720.setCheckable(True)
if pcl_config["stream_height"] == 640:
self.action640x480.setChecked(True)
else:
self.action1280x720.setChecked(True)
self.fpsGroup = QActionGroup(self)
self.fpsGroup.addAction(self.action15)
self.fpsGroup.addAction(self.action30)
self.fpsGroup.setExclusive(True)
self.action15.setCheckable(True)
self.action30.setCheckable(True)
if pcl_config["stream_fps"] == 15:
self.action15.setChecked(True)
else:
self.action30.setChecked(True)
# Help Menu
self.actionAbout.triggered.connect(self.about_dialog)
self.actionDocumentation.triggered.connect(
lambda: self.open_URL(HELP_DOCUMENTATION_URL))
def open_URL(self, url):
"""
Opens specified URL
Args:
url ([string]): [The URL to open, can be formatted using QUrl]
"""
webbrowser.open(url)
def set_res(self, width, height):
"""
Set resolution of the stream
Args:
width (int): Width of the stream
height (int): Height of the stream
"""
self.configure_stream(width, height, pcl_config["stream_fps"])
def set_fps(self, fps):
"""
Set FPS of the stream
Args:
fps (int): Frames per Second of the stream
"""
self.configure_stream(pcl_config["stream_width"],
pcl_config["stream_height"], fps)
def set_view(self, view_type):
"""
Set View Type of the window
Args:
view_type (string): View type of the window, monitor or vr
"""
log.info("View Type Changed to " + view_type)
def start_stream(self):
"""
Start the camera stream.
Creates a seperate process for the stream, and interacts with the frames through shared memory.
Uses a QTimer to maintain a stable frame rate.
"""
# grab config variables
STREAM_HEIGHT = pcl_config["stream_height"]
STREAM_WIDTH = pcl_config["stream_width"]
STREAM_FPS = pcl_config["stream_fps"]
# set up the label
camera_size_policy = QSizePolicy(STREAM_WIDTH, STREAM_HEIGHT)
self.camera_label.setSizePolicy(camera_size_policy)
# create shared memory to hold a single frame for inter process communication,
# wrap it in a numpy array
nbyte_per_frame = STREAM_WIDTH * STREAM_HEIGHT * FRAME_NCHANNELS # bytes in a frame
log.debug("Allocating %i x %i bytes of shared memory" %
(SM_BUF_SIZE, nbyte_per_frame))
self.frame_shm = shared_memory.SharedMemory(create=True,
size=nbyte_per_frame *
SM_BUF_SIZE)
shm_shape = (SM_BUF_SIZE, STREAM_HEIGHT, STREAM_WIDTH, FRAME_NCHANNELS)
self.frame_buffer = np.ndarray(shm_shape,
dtype=np.uint8,
buffer=self.frame_shm.buf)
# spawn the child depth cam process
self.frame_in_use = Value("i", False)
self.pending_frame = Value("i", False)
read_pipe, write_pipe = multiprocessing.Pipe()
self.camera_communication_pipe = write_pipe
self.depth_cam_proc = Process(
target=generate_frames,
args=(self.frame_shm.name, shm_shape, STREAM_HEIGHT, STREAM_WIDTH,
STREAM_FPS, self.frame_in_use, self.pending_frame, read_pipe,
self.is_streaming),
)
self.depth_cam_proc.start()
log.info("Created depth camera process, pId = %i" %
self.depth_cam_proc.pid)
# create a QTimer to manage frame updates
self.fps_timer = QtCore.QTimer()
self.fps_timer.setTimerType(QtCore.Qt.PreciseTimer)
self.fps_timer.timeout.connect(self.update_stream)
self.fps_timer.setInterval(round(1000.0 / STREAM_FPS))
self.fps_timer.start()
log.debug("Timer limiting FPS to %i" % STREAM_FPS)
# just for some stats
self.frameUpdateCount = 0
self.stream_start_time = time.time()
# for camera depending on accelerometer
self.waitingForLevel = False
def configure_stream(self, width, height, fps):
# End exisiting stream
pcl_config["stream_height"] = height
pcl_config["stream_width"] = width
pcl_config["stream_fps"] = fps
# Tear down camera process
self.end_stream()
# Start stream with new values
self.start_stream()
log.info("The stream configuration is as follows:\n Resolution: " +
str(height) + "x" + str(width) + "\nFPS: " + str(fps))
def update_stream(self):
"""
Updates the GUI to display the current video frame.
"""
self.frame_in_use.value = True
pix = frame_to_pixmap(self.frame_buffer[0])
self.camera_label.setPixmap(pix)
self.frame_in_use.value = False
self.frameUpdateCount += 1
def take_picture(self):
"""
Notifies camera process to save the next frame.
Disables the take picture button & posts the image name and experiment path onto the shared pipe.
(SUS) Waits till image is saved to enable the button again.
"""
if not self.is_streaming.value:
log.info("Cant save pic - no stream !!")
return
# disable take pic btn until its done
self.take_picture_button.setEnabled(False)
if not within_tolerance(self.x, self.y, LEVEL_TOLERANCE):
log.info("Waiting till camera is level to take the picture")
self.waitingForLevel = True
return
# write save image info to pipe
plot_num_str = str(self.current_plot_number).zfill(PLOT_NUMBER_PADDING)
img_name = "%s_%s" % (self.experiment_name, plot_num_str)
log.debug("Queueing image <%s> to be saved..." % img_name)
self.pending_frame.value = True
self.camera_communication_pipe.send((self.experiment_path, img_name))
# wait for image to be saved
while (self.pending_frame.value):
pass
self.update_metadata()
self.save_metadata()
# resume normal operations
log.debug("Image <%s> successfully saved" % img_name)
self.update_plot_number(self.current_plot_number + 1)
self.take_picture_button.setEnabled(True)
self.alert.play()
self.waitingForLevel = False
def start_accelerometer(self):
"""
Creates Phidget22 accelerometer and polls it at a fixed rate using a QTimer.
"""
# set up the target image
self.setup_target()
# create the accelerometer, and wait 1 sec for connection
# TODO does the open wait for attachment throw an error
try:
self.accelerometer = Accelerometer()
self.accelerometer.openWaitForAttachment(1000)
log.info("Created accelerometer")
except:
log.warning("There is no accelerometer connected")
self.accelerometer = None
return
# create QTimer to periodically monitor the acceleration
self.accelerometer_timer = QtCore.QTimer()
self.accelerometer_timer.setTimerType(QtCore.Qt.PreciseTimer)
self.accelerometer_timer.timeout.connect(self.update_target)
self.accelerometer_timer.setInterval(ACCELEROMETER_PERIOD_MS)
self.accelerometer_timer.start()
log.debug("Timer limiting accelerometer update frequency to %f Hz" %
(1000.0 / ACCELEROMETER_PERIOD_MS))
def setup_target(self):
"""
Sets up the target graphic of the GUI.
"""
self.wrapper = QWidget()
self.target_layout = QGridLayout()
self.target_layout.addWidget(self.wrapper)
self.target_widget.setLayout(self.target_layout)
self.target = Target()
self.target.pixmap = QPixmap(TARGET_ICON_PATH)
self.target.setGeometry(40, 0, 150, 150)
self.target.setParent(self.wrapper)
self.target.show()
def update_target(self):
"""
Update the coordinate on the GUI's target.
"""
# TODO try-catch error
acceleration = self.accelerometer.getAcceleration()
self.x = acceleration[0]
self.y = acceleration[1]
self.target.coordinate = QPointF(self.x, self.y)
if within_tolerance(self.x, self.y, LEVEL_TOLERANCE):
if self.waitingForLevel:
self.take_picture()
self.camera_level_label.setText("Camera is Level")
self.camera_level_label.setStyleSheet("color: green;")
else:
self.camera_level_label.setText("Camera is not Level")
self.camera_level_label.setStyleSheet("color: red;")
def about_dialog(self):
"""
Open the "New Experiment" Dialog and connect methods to update file name and plot number labels.
"""
about_page = AboutPage()
about_page.exec_()
about_page.show()
def new_experiment_dialog(self):
"""
Open the "New Experiment" Dialog and connect methods to update file name and plot number labels.
"""
new_exp_page = NewExperimentPage()
new_exp_page.expirementCreated.connect(self.update_experiment)
new_exp_page.changePlotNumber.connect(self.update_plot_number)
new_exp_page.exec_()
new_exp_page.show()
def browse_files(self):
"""
Opens up experiment folder directory so that an existing experiment can be chosen
"""
loaded_filename = QFileDialog.getExistingDirectory(
self, 'Select Experiment Folder', PCL_EXP_PATH)
self.update_experiment(basename(loaded_filename))
@pyqtSlot(str)
def update_experiment(self, new_exp_name):
"""
Updates the working experiment.
Modifys all relevant variables and paths.
Sets up metadata to be periodically saved to file.
Enables button if experiment is set.
Args:
new_exp_name (str): The new experiment name
"""
# make sure this is a valid experiment before updating
new_exp_path = os.path.join(PCL_EXP_PATH, new_exp_name)
if not self.validate_experiment(new_exp_path):
return
self.experiment_name = new_exp_name
self.file_name_label.setText(self.experiment_name)
self.experiment_path = new_exp_path
# save old metadata before opening new experiment
self.save_metadata()
metadata_path = os.path.join(self.experiment_path, "Metadata",
"%s.json" % self.experiment_name)
self.metadata = Metadata(metadata_path)
last_index = self.metadata.get_last_index()
if last_index != -1:
self.update_plot_number(last_index + 1)
self.take_picture_button.setEnabled(True)
log.info("Opened experiment %s" % self.experiment_name)
def validate_experiment(self, exp_path):
"""
Validates experiment at the given path.
If no experiment exists or the experiment structure is faulty,
Displays an error message dialog.
Args:
exp_path (path): path to a plotcam experiment
Returns:
bool: true if experiment is valid
"""
# TODO would u want to create RGB and depth folders if they r missing
valid = True
errorMsg = ""
if not os.path.exists(exp_path):
valid = False
errorMsg = "No experiment at %s" % exp_path
elif not os.path.exists(os.path.join(exp_path, "Depth")):
valid = False
errorMsg = "Experiment missing depth data directory"
elif not os.path.exists(os.path.join(exp_path, "RGB")):
valid = False
errorMsg = "Experiment missing rgb data directory"
# dont need to validate metadata, the path will be created if it doenst exist on update exp
if not valid:
# error pop up
log.info(errorMsg)
pass
return valid
@pyqtSlot(int)
def update_plot_number(self, new_plot_number):
"""
Updates plot number.
Args:
new_plot_number (int): The new plot number
"""
self.plot_number_label.setText(
"Plot #: " + str(new_plot_number).zfill(PLOT_NUMBER_PADDING))
self.current_plot_number = new_plot_number
def update_metadata(self):
"""
Updates metadata with new entry.
"""
num = str(self.current_plot_number).zfill(PLOT_NUMBER_PADDING)
t = datetime.now().strftime('%H:%M:%S')
date = datetime.now().strftime('%d/%m/%Y')
name = self.experiment_name
self.metadata.add_entry(num, t, date, self.x, self.y, name)
def save_metadata(self):
"""
Saves the metadata file to disk.
"""
if self.metadata is None:
return
log.info("Saving metadata")
self.metadata.save()
def end_stream(self):
if self.depth_cam_proc:
log.debug("Stream's Average FPS: %.2f" %
(self.frameUpdateCount /
(time.time() - self.stream_start_time)))
# end depth cam process and wait for it to complete
# TODO end the fps Qtimer
self.is_streaming.value = False
self.frame_in_use.value = False
self.depth_cam_proc.join()
# free shared memory
self.frame_shm.close()
self.frame_shm.unlink()
log.info("Terminated camera process")
def closeEvent(self, event):
"""
Overrides systems "self.close" method so that the program can terminate properly.
Teardown all running threads and processes.
"""
# tear down accelerometer
if self.accelerometer:
pass
# tear down camera process
self.end_stream()
log.info("PlotCamLite says goodbye :[")
