def add_config(self, title, choices, single_choice=True):
'''
create a UI element for selecting options for one variation
and put it in a scrollArea
'''
scroll = QScrollArea()
group_box = QGroupBox(title)
group_box.setFlat(True)
layout = QVBoxLayout()
if len(choices) > 5 and single_choice:
combo_box = QComboBox(group_box)
for obj in choices:
combo_box.addItem(obj)
layout.addWidget(combo_box)
else:
for obj in choices:
if single_choice:
layout.addWidget(QRadioButton(obj, group_box))
else:
layout.addWidget(QCheckBox(obj, group_box))
layout.addStretch(1)
group_box.setLayout(layout)
scroll.setWidget(group_box)
scroll.setWidgetResizable(True)
return group_box, scroll
def add_buttons(self, colors):
vbx = QVBoxLayout()
i = 1
for color in colors:
but = QPushButton('Color {} ({})'.format(color, i))
but.clicked.connect(self.clicked(color))
but_short = QShortcut(QKeySequence("Ctrl+" + str(i)), self)
but_short.activated.connect(self.clicked(color))
i += 1
vbx.addWidget(but)
vbx.addStretch(1)
self.color_buttons.setLayout(vbx)
def __init__(self):
super(MyWidget, self).__init__()
self.brd = CvBridge()
self.view = RGB
ui_file = os.path.join(
rospkg.RosPack().get_path('balloon_color_picker'), 'resource',
'ColorPlugin.ui')
rospy.loginfo('uifile {}'.format(ui_file))
# self.log_info('uifile {}'.format(ui_file))
loadUi(ui_file, self)
# Give QObjects reasonable names
self.setObjectName('ColorPluginUi')
self.uav_name = os.environ['UAV_NAME']
self.orig_h = 920
self.orig_w = 1080
self.hist_hsv_orig_h = 180
self.hist_hsv_orig_w = 256
self.hist_lab_orig_h = 256
self.hist_lab_orig_w = 256
self.hist_status = HSV
self.select_status = HIST_SELECTION
self.crop_stat = IMG
self.hist_mask = np.zeros([self.hist_hsv_orig_h, self.hist_hsv_orig_w])
self.hist_mask_lab = np.zeros(
[self.hist_lab_orig_h, self.hist_lab_orig_w])
self.cur_hist_hs = None
self.cur_hist_ab = None
self.selected_count = 0
# ROS services
# #{ ros services
self.sigma_caller = rospy.ServiceProxy('change_sigma', ChangeSigma)
self.sigma_lab_caller = rospy.ServiceProxy('change_sigma_lab',
ChangeSigmaLab)
self.caller = rospy.ServiceProxy('capture', Capture)
self.capture_cropped_srv = rospy.ServiceProxy('capture_cropped',
CaptureCropped)
self.get_count = rospy.ServiceProxy('get_count', GetCount)
self.clear_count = rospy.ServiceProxy('clear_count', ClearCount)
self.get_config = rospy.ServiceProxy('get_config', GetConfig)
self.get_params = rospy.ServiceProxy('get_params', Params)
self.freeze_service = rospy.ServiceProxy('freeze', Freeze)
self.update_service = rospy.ServiceProxy('change_obd', UpdateObd)
self.change_callback = rospy.ServiceProxy('change_callback',
ChangeCallback)
self.capture_hist = rospy.ServiceProxy('capture_hist', CaptureHist)
# #} end of ros services rospy.wait_for_service('capture')
rospy.loginfo(
'waiting for service "get_params" from computation module \n if this is more than 5 secs check for topic remapping'
)
# self.log_info('waiting for service')
rospy.loginfo('uav_name {}'.format(os.environ['UAV_NAME']))
# self.log_info('uav_name {}'.format(os.environ['UAV_NAME']))
rospy.wait_for_service('get_params')
self.config_path, self.save_path, self.circled_param, self.circle_filter_param, self.circle_luv_param, self.object_detect_param, self.save_to_drone = self.set_params(
)
# SUBS
# #{ ros subs
self.balloon_sub = rospy.Subscriber(self.circled_param,
RosImg,
self.img_callback,
queue_size=1)
self.filter_sub = rospy.Subscriber(self.circle_filter_param,
RosImg,
self.filter_callback,
queue_size=1)
self.filter_luv = rospy.Subscriber(self.circle_luv_param,
RosImg,
self.luv_callback,
queue_size=1)
self.obj_det_sb = rospy.Subscriber(self.object_detect_param,
RosImg,
self.obj_det_callback,
queue_size=1)
# self.hsv = message_filters.Subscriber(self.circle_filter_param, RosImg)
# self.luv = message_filters.Subscriber(self.circle_luv_param, RosImg)
# self.ts = message_filters.ApproximateTimeSynchronizer([self.luv, self.hsv], 1, 0.5)
# self.ts.registerCallback(self.both_callback)
# #} end of ros subs
self.colors = self.load_config(self.config_path)
self.add_buttons(self.colors)
# # DEFAULT IMAGE
# img = cv2.imread('/home/mrs/balloon_workspace/src/ros_packages/balloon_color_picker/data/blue.png')
# cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
# h,w,c = img.shape
# q_img = QImage(img.data, w,h,3*w, QImage.Format_RGB888)
# q = QPixmap.fromImage(q_img)
#DIRECTORY
#default
self.directory.setText(self.save_path + "Red.yaml")
self.color_name = "red"
self.save_button.clicked.connect(self.save_config)
#PLOT
self.figure = Figure()
self.figure_luv = Figure()
self.canvas = FigureCanvas(self.figure)
self.canvas_luv = FigureCanvas(self.figure_luv)
self.canvas.setParent(self.inner)
# self.toolbar = NavigationToolbar(self.canvas,self)
# self.toolbar_luv = NavigationToolbar(self.canvas_luv,self)
# self.toolbar.setParent(self.inner)
# self.toolbar_luv.setParent(self.inner_luv)
self.canvas_luv.setParent(self.inner_luv)
self.inner_luv.hide()
self.inner_luv_hist.hide()
#SLIDER CONFIG
# #{ slider config
self.sigma_slider.setRange(0, 100)
self.sigma_slider.setSingleStep(1)
self.sigma_slider.setValue(6)
self.sigma_slider.valueChanged.connect(self.slider_event)
self.sigma_slider_s.setRange(0, 100)
self.sigma_slider_s.setSingleStep(1)
self.sigma_slider_s.setValue(6)
self.sigma_slider_s.valueChanged.connect(self.slider_event)
self.sigma_slider_v.setRange(0, 100)
self.sigma_slider_v.setSingleStep(1)
self.sigma_slider_v.setValue(80)
self.sigma_slider_v.valueChanged.connect(self.slider_event)
self.sigma_slider_lab.setRange(0, 100)
self.sigma_slider_lab.setSingleStep(1)
self.sigma_slider_lab.setValue(80)
self.sigma_slider_lab.valueChanged.connect(self.slider_event_lab)
self.sigma_slider_a.setRange(0, 100)
self.sigma_slider_a.setSingleStep(1)
self.sigma_slider_a.setValue(6)
self.sigma_slider_a.valueChanged.connect(self.slider_event_lab)
self.sigma_slider_b.setRange(0, 100)
self.sigma_slider_b.setSingleStep(1)
self.sigma_slider_b.setValue(6)
self.sigma_slider_b.valueChanged.connect(self.slider_event_lab)
# #} end of slider config
# #{ font configs
#SIGMA TEXT
font = self.font()
font.setPointSize(16)
self.sigma_value.setFont(font)
self.sigma_value_s.setFont(font)
self.sigma_value_v.setFont(font)
self.sigma_value_lab.setFont(font)
self.sigma_value_a.setFont(font)
self.sigma_value_b.setFont(font)
#IMAGE COUNT TEXT
self.image_count.setFont(font)
#BOX FOR BUTTONS font
# self.color_buttons.setFont(font)
font.setPointSize(14)
self.sigma_value.setFont(font)
self.log_text.setFont(font)
#LAB HSV TEXT
font.setPointSize(23)
self.label_lab.setFont(font)
self.label_lab.hide()
self.label_hsv.setFont(font)
self.label_hsv.hide()
# #} end of font configs
# BUTTONS
self.change.clicked.connect(self.switch_view_hsv)
self.change_both.clicked.connect(self.switch_view_both)
self.change_luv.clicked.connect(self.switch_view_luv)
self.change_object.clicked.connect(self.switch_view_object_detect)
self.capture_button.clicked.connect(self.capture)
self.clear_button.clicked.connect(self.clear)
self.freeze_button.clicked.connect(self.freeze)
self.update_button.clicked.connect(self.update_obd)
# self.wdg_img.setPixmap(q)
# self.box_layout.addWidget(self.toolbar)
# self.inner.box_layout.addWidget(self.canvas)
#shortcuts
self.short_capture = QShortcut(QKeySequence("C"), self)
self.short_capture.activated.connect(self.capture)
self.short_hsv = QShortcut(QKeySequence("1"), self)
self.short_hsv.activated.connect(self.switch_view_hsv)
self.short_lab = QShortcut(QKeySequence("2"), self)
self.short_lab.activated.connect(self.switch_view_luv)
self.short_object_detect = QShortcut(QKeySequence("3"), self)
self.short_object_detect.activated.connect(
self.switch_view_object_detect)
self.short_object_detect_update = QShortcut(QKeySequence("U"), self)
self.short_object_detect_update.activated.connect(self.update_obd)
self.short_both = QShortcut(QKeySequence("4"), self)
self.short_both.activated.connect(self.switch_view_both)
self.short_save = QShortcut(QKeySequence("S"), self)
self.short_save.activated.connect(self.save_config)
self.short_clear = QShortcut(QKeySequence("N"), self)
self.short_clear.activated.connect(self.clear)
self.short_freeze = QShortcut(QKeySequence("F"), self)
self.short_freeze.activated.connect(self.freeze)
vbx = QVBoxLayout()
but_hsv = QRadioButton()
but_hsv.setText('HSV')
but_hsv.setChecked(True)
self.color_space = 'HSV'
but_hsv.clicked.connect(self.set_colorspace_hsv)
vbx.addWidget(but_hsv)
but_lab = QRadioButton()
but_lab.setText('LAB')
but_lab.clicked.connect(self.set_colorspace_lab)
vbx.addWidget(but_lab)
vbx.addStretch(1)
self.radio_buttons.setLayout(vbx)
vbx_method = QVBoxLayout()
but_lut = QRadioButton()
but_lut.setText('LUT')
but_lut.setChecked(False)
but_lut.clicked.connect(self.set_method_lut)
vbx_method.addWidget(but_lut)
but_thr = QRadioButton()
but_thr.setText('Threshold')
but_thr.setChecked(True)
but_thr.clicked.connect(self.set_method_thr)
vbx_method.addWidget(but_thr)
vbx.addStretch(1)
self.radio_buttons_method.setLayout(vbx_method)
self.load_method = 'THR'
# self.mousePressEvent.connect(self.mousePressEvent)
self.plotted = False
self._rubber = None
self.frozen = False
class EventTransmissionGUI(Plugin):
"""
GUI to send events from User to logic state machine
"""
def __init__(self, context):
"""
Create Qt GUI using the event file
"""
super(EventTransmissionGUI, self).__init__(context)
self.setObjectName('ManualEventTriggerGUI')
parser = ArgumentParser()
# Add argument(s) to the parser.
args = self._parse_args(context.argv())
## Create Event trigger
self.event_trigger = RosEventTrigger(args.event_file)
## Parent container to store buttons, textboxes
self._container = QTabWidget()
## Tab to display robot system, state machine status
## and command buttons without any arguments
self._status_tab = QWidget()
## Tab to display positionyaw and velocityyaw commands
## and sliders for them
self._additional_commands_tab = QWidget()
# Set title of the parent container window
self._status_tab.setWindowTitle(self.event_trigger.event_manager_name)
## Layout for status tab
self._layout = QVBoxLayout()
self._status_tab.setLayout(self._layout)
# Create Textboxes and add to Layout
self._layout.addWidget(QLabel('State Machine State'))
## Textbox to show sytem status
self.system_status_textbox = QTextEdit()
self.system_status_textbox.setReadOnly(True)
self._layout.addWidget(self.system_status_textbox)
# Define and connect buttons
self._layout.addWidget(QLabel('Event Triggers'))
## Continer to store event triggering buttons
self.button_container = QWidget()
## List of push buttons to trigger events
self.push_buttons = list()
## Layout for the push buttons
self.button_layout = QGridLayout()
self.button_container.setLayout(self.button_layout)
button_index = 0
for event_name in self.event_trigger.event_names_list:
self.push_buttons.append(QPushButton(event_name))
partial_fun = partial(self.event_trigger.triggerEvent,
event_name=event_name)
self.push_buttons[-1].clicked.connect(partial_fun)
row, col = self.get_row_col(button_index, args.grid_cols)
self.button_layout.addWidget(self.push_buttons[-1], row, col)
button_index += 1
self._layout.addWidget(self.button_container)
## Layout for additional commands tab
self._additional_commands_layout = QVBoxLayout()
self._additional_commands_tab.setLayout(
self._additional_commands_layout)
# Create height slider
self._additional_commands_layout.addWidget(
QLabel('Pose Command Height (m)'))
## Height slider to adjust z coordinate for pose command
## \todo Matt: Load slider settings from param file
self.height_slider = self.createSlider(1.0, 20.0, 2.0, 1.0)
self._additional_commands_layout.addWidget(self.height_slider)
## Add button for triggering pose command
## Container for pose event related objects: slide etc
## \todo Matt: Reset slider value based on current quad height
self.pose_command_container = QWidget()
## Pose command layout
self.pose_command_layout = QGridLayout()
self.pose_command_container.setLayout(self.pose_command_layout)
## x pose label to display position command from rviz to user
self.pose_x = QLabel('x: -')
## y pose label to display position command from rviz to user
self.pose_y = QLabel('y: -')
## z pose label to display position command from rviz to user
self.pose_z = QLabel("z: {0:.2f}".format(self.height_slider.value()))
self.height_slider.valueChanged.connect(
partial(self.updateLabel,
header="z",
label=self.pose_z,
slider=self.height_slider))
self.pose_command_layout.addWidget(self.pose_x, 0, 0)
self.pose_command_layout.addWidget(self.pose_y, 0, 1)
self.pose_command_layout.addWidget(self.pose_z, 0, 2)
## Button to send the pose command to state machine as poseyaw event
self.send_pose_command_button = QPushButton("Send Pose Command")
self.send_pose_command_button.clicked.connect(
self.poseCommandButtonCallback)
self.pose_command_layout.addWidget(self.send_pose_command_button, 0, 3)
self._additional_commands_layout.addWidget(self.pose_command_container)
## Pose command container to store pose from Rviz and send to state
## machine
self.pose_command = None
## Sliders for setting vx,vy,vz, yaw
self.velocity_sliders = []
## Labels vx,vy,vz,yaw as an array
self.velocity_command_labels = []
self._additional_commands_layout.addWidget(
QLabel('Velocity Command (m/s), Yaw (deg)'))
for i in range(3):
self.velocity_sliders.append(
self.createSlider(-20.0, 20.0, 0.0, 1.0))
self._additional_commands_layout.addWidget(
self.velocity_sliders[i])
# Slider for yaw
self.velocity_sliders.append(
self.createSlider(-180.0, 180.0, 0.0, 10.0))
self._additional_commands_layout.addWidget(self.velocity_sliders[-1])
## Add button for triggering velocity yaw
self.velocity_command_container = QWidget()
## Velocity command layout
self.velocity_command_layout = QGridLayout()
self.velocity_command_container.setLayout(self.velocity_command_layout)
for i, axis_label in enumerate(['x', 'y', 'z']):
# label to display velocity command from rviz to user
self.velocity_command_labels.append(
QLabel("v{0}: {1:.2f}".format(
axis_label, self.velocity_sliders[i].value() / 10.0)))
self.velocity_sliders[i].valueChanged.connect(
partial(self.updateLabel,
header="v" + str(i),
label=self.velocity_command_labels[i],
slider=self.velocity_sliders[i],
scale=10.0))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[i], 0, i)
# Label for yaw
self.velocity_command_labels.append(
QLabel("Yaw: {0:.2f}".format(self.velocity_sliders[-1].value())))
self.velocity_sliders[-1].valueChanged.connect(
partial(self.updateLabel,
header="Yaw",
label=self.velocity_command_labels[-1],
slider=self.velocity_sliders[-1]))
self.velocity_command_layout.addWidget(
self.velocity_command_labels[-1], 0, 3)
## Button to send the pose command to state machine as poseyaw event
self.send_velocity_command_button = QPushButton(
"Send Velocity Command")
self.send_velocity_command_button.clicked.connect(
self.velocityCommandButtonCallback)
self.velocity_command_layout.addWidget(
self.send_velocity_command_button, 0, 4)
self._additional_commands_layout.addWidget(
self.velocity_command_container)
self._additional_commands_layout.addStretch()
self._container.addTab(self._status_tab, "StatusBasicCommands")
self._container.addTab(self._additional_commands_tab,
"AdditionalCommands")
context.add_widget(self._container)
# Add textboxes to update hooks from eventTrigger class
# Define Partial callbacks
systemStatusCallback = partial(self.updateStatus,
text_box=self.system_status_textbox)
# Connect Event Triggers
self.event_trigger.status_signal.connect(systemStatusCallback)
self.event_trigger.pose_command_signal.connect(
self.poseCommandCallback)
def createSlider(self, minimum, maximum, default_value, tick_interval):
"""
Create a QtSlider with specified properties
Parameters:
@param minimum Minimum value for slider
@param maximum Maximum value for slider
@param default_value Initial value the slider is set to
@param tick_inverval Integer value specifying difference between
successive ticks
@return Slider value
"""
slider = QSlider(Qt.Horizontal)
slider.setMinimum(minimum)
slider.setMaximum(maximum)
slider.setValue(default_value)
slider.setTickPosition(QSlider.TicksBelow)
slider.setTickInterval(tick_interval)
return slider
def _parse_args(self, argv):
"""
Parse extra arguments when plugin is deployed in standalone mode
"""
parser = argparse.ArgumentParser(prog='aerial_autonomy',
add_help=False)
EventTransmissionGUI.add_arguments(parser)
return parser.parse_args(argv)
@staticmethod
def add_arguments(parser):
"""
Notify rqt_gui that this plugin can parse these extra arguments
"""
group = parser.add_argument_group(
'Options for aerial autonomy gui plugin')
group.add_argument("-e",
"--event_file",
type=str,
default='',
help="Event file")
group.add_argument("-c",
"--grid_cols",
type=int,
default=3,
help="Number of columns in grid")
def get_row_col(self, button_index, ncols):
"""
Automatically find the row and col to add the button
to in a grid based on index of the button
"""
col_index = button_index % ncols
row_index = int((button_index - col_index) / ncols)
return (row_index, col_index)
def poseCommandCallback(self, pose):
"""
Saves pose command and updates command display
"""
self.pose_command = pose
self.pose_x.setText("x: {0:.2f}".format(
self.pose_command.pose.position.x))
self.pose_y.setText("y: {0:.2f}".format(
self.pose_command.pose.position.y))
def poseCommandButtonCallback(self):
"""
Publishes stored pose command after setting height from slider
"""
if self.pose_command:
self.pose_command.pose.position.z = self.height_slider.value()
self.event_trigger.triggerPoseCommand(self.pose_command)
# Reset pose command to avoid accidental triggering
self.pose_command = None
self.pose_x.setText('x: -')
self.pose_y.setText('y: -')
else:
print "No pose command to trigger"
def velocityCommandButtonCallback(self):
"""
Publishes stored velocity command
"""
velocity_command = VelocityYaw()
velocity_command.vx = self.velocity_sliders[0].value() / 10.0
velocity_command.vy = self.velocity_sliders[1].value() / 10.0
velocity_command.vz = self.velocity_sliders[2].value() / 10.0
velocity_command.yaw = self.velocity_sliders[3].value() * np.pi / 180.0
self.event_trigger.triggerVelocityCommand(velocity_command)
def updateLabel(self, header, label, slider, scale=1):
"""
Updates height label based on slider value
"""
label.setText(header + ": {0:.2f}".format(slider.value() / scale))
def updateStatus(self, status, text_box):
"""
Generic placeholder function to update text box
"""
if not sip.isdeleted(text_box):
text_box.setHtml(status)
doc_size = text_box.document().size()
text_box.setFixedHeight(doc_size.height() + 10)
