class MissionServer(object):
def __init__(self, plan_names, master_timeout=None):
self._plans = PlanSet(plan_names)
self._master_timeout = master_timeout
self._sm = None
self._shared = uf_smach.util.StateSharedHandles()
self._pub = rospy.Publisher('mission/plans', PlansStamped)
self._srv = rospy.Service('mission/modify_plan', ModifyPlan, self._modify_plan)
self._run_srv = actionlib.SimpleActionServer('mission/run', RunMissionsAction,
self.execute, False)
self._run_srv.register_preempt_callback(self._on_preempt)
self._run_srv.start()
self._tim = rospy.Timer(rospy.Duration(.1), lambda _: self._publish_plans())
self._kill_listener = KillListener(self._on_preempt)
def get_plan(self, plan):
return self._plans.get_plan(plan)
def execute(self, goal):
self._sm = self._plans.make_sm(self._shared, self._master_timeout)
sis = smach_ros.IntrospectionServer('mission_planner', self._sm, '/SM_ROOT')
sis.start()
print 'Waiting for unkill'
while self._kill_listener.get_killed():
rospy.sleep(.1)
outcome = self._sm.execute()
sis.stop()
self._shared['moveto'].cancel_goal()
if outcome == 'succeeded':
self._run_srv.set_succeeded(RunMissionsResult(outcome))
else:
self._run_srv.set_preempted()
def _on_preempt(self):
if self._sm is not None:
self._sm.request_preempt()
def _publish_plans(self):
self._pub.publish(PlansStamped(
header=Header(stamp=rospy.Time.now()),
plans=[Plan(name=name,
entries=[uf_smach.msg.PlanEntry(entry.mission, rospy.Duration(entry.timeout),
entry.contigency_plan, entry.path, entry.dist)
for entry in self._plans.get_plan(name)])
for name in self._plans.get_plans()],
available_missions=get_missions()))
def _modify_plan(self, req):
if req.plan not in self._plans.get_plans():
return None
plan = self._plans.get_plan(req.plan)
entry = PlanEntry(req.entry.mission,
req.entry.timeout.to_sec(),
req.entry.contigency_plan if len(req.entry.contigency_plan) > 0 else None,
req.entry.path if req.entry.path != 'none' else None,
req.entry.dist)
if req.operation == ModifyPlanRequest.INSERT:
if req.pos > len(plan):
return None
plan.insert(req.pos, entry)
elif req.operation == ModifyPlanRequest.REMOVE:
if req.pos >= len(plan):
return None
del plan[req.pos]
elif req.operation == ModifyPlanRequest.REPLACE:
if req.pos >= len(plan):
return None
plan[req.pos] = entry
else:
return None
return ()
class PropaGatorGUI(Plugin):
def __init__(self, contex):
super(PropaGatorGUI, self).__init__(contex)
# Initilize variables
self._float_status = False
self.last_odom_msg = Odometry()
self._current_controller = 'Controller: Unknown'
self._controller = ''
self._controllers = []
# Assign a name
self.setObjectName('PropaGatorGUI')
# Create a widget
self._widget = QWidget()
loadUi(os.path.join(cwd, 'propagatorgui.ui'), self._widget)
self._widget.setObjectName('PropaGatorGUI')
contex.add_widget(self._widget)
# Grab all the children from the widget
self._kill_label = self._widget.findChild(QLabel, 'kill_label')
self._float_label = self._widget.findChild(QLabel, 'float_label')
self._controller_label = self._widget.findChild(
QLabel, 'controller_label')
self._odom_x_label = self._widget.findChild(QLabel, 'odom_x_label')
self._odom_y_label = self._widget.findChild(QLabel, 'odom_y_label')
self._odom_yaw_label = self._widget.findChild(QLabel, 'odom_yaw_label')
self._odom_d_x_label = self._widget.findChild(QLabel, 'odom_d_x_label')
self._odom_d_y_label = self._widget.findChild(QLabel, 'odom_d_y_label')
self._odom_d_yaw_label = self._widget.findChild(
QLabel, 'odom_d_yaw_label')
self._placeholder_label = self._widget.findChild(
QLabel, 'placeholder_label')
self._kill_push_btn = self._widget.findChild(QPushButton,
'kill_push_btn')
self._float_push_btn = self._widget.findChild(QPushButton,
'float_push_btn')
self._controller_combo_box = self._widget.findChild(
QComboBox, 'controller_combo_box')
# Load images
self._green_indicator = QPixmap(
os.path.join(cwd, 'green_indicator.png'))
self._red_indicator = QPixmap(os.path.join(cwd, 'red_indicator.png'))
self._placeholder_image = QPixmap(os.path.join(cwd, 'placeholder.png'))
# Set up ROS interfaces
self._kill_listener = KillListener()
self._kill_broadcaster = KillBroadcaster(
id='PropaGator GUI', description='PropaGator GUI kill')
self._odom_sub = rospy.Subscriber('/odom', Odometry, self._odom_cb)
self._float_sub = rospy.Subscriber('/controller/float_status', Bool,
self._float_cb)
self._float_proxy = rospy.ServiceProxy('/controller/float_mode',
FloatMode)
self._current_controller_sub = rospy.Subscriber(
'/controller/current_controller', String,
self._current_controller_cb)
self._get_all_controllers_proxy = rospy.ServiceProxy(
'/controller/get_all_controllers', get_all_controllers)
self._request_controller_proxy = rospy.ServiceProxy(
'/controller/request_controller', request_controller)
# Connect push buttons
self._kill_push_btn.toggled.connect(self._on_kill_push_btn_toggle)
self._float_push_btn.toggled.connect(self._on_float_push_btn_toggle)
# Connect combo boxes
self._controller_combo_box.activated[str].connect(
self._controller_combo_box_cb)
# Set up update timer at 10Hz
# A Qt timer is used instead of a ros timer since Qt components are updated
self.update_timer = QTimer()
self.update_timer.timeout.connect(self._onUpdate)
self.update_timer.start(100)
# Temp
self._placeholder_label.setPixmap(self._placeholder_image)
# Everything needs to be turned off here
def shutdown_plugin(self):
self.update_timer.stop()
self._odom_sub.unregister()
del self._odom_sub
self._float_sub.unregister()
del self._float_sub
# Kill broadcaster is not cleared, the user should unkill before closing the GUI
del self._kill_broadcaster
del self._kill_listener
# Subscriber callbacks
# Since this is in a different thread it is possible and likely that
# the drawing thread will try and draw while the text is being changed
# this causes all kinds of mahem such as segmentation faults, double free, ...
# To prevent this from hapening this thread updates only none QT variables
# described here http://wiki.ros.org/rqt/Tutorials/Writing%20a%20Python%20Plugin
def _odom_cb(self, msg):
self.last_odom_msg = msg
def _float_cb(self, status):
self._float_status = status.data
def _current_controller_cb(self, controller):
self._controller = controller.data
self._current_controller = 'Controller: ' + controller.data
def _odom_update(self):
pos = self.last_odom_msg.pose.pose.position
yaw = quat_to_rotvec(
xyzw_array(self.last_odom_msg.pose.pose.orientation))[2]
vel = self.last_odom_msg.twist.twist.linear
dYaw = self.last_odom_msg.twist.twist.angular.z
self._odom_x_label.setText('X: %3.3f' % pos.x)
self._odom_y_label.setText('Y: %3.3f' % pos.y)
self._odom_yaw_label.setText('Yaw: %3.3f' % yaw)
self._odom_d_x_label.setText('dX: %3.3f' % vel.x)
self._odom_d_y_label.setText('dY: %3.3f' % vel.y)
self._odom_d_yaw_label.setText('dYaw: %3.3f' % dYaw)
# Push btn callbacks
def _on_kill_push_btn_toggle(self, checked):
if checked:
self._kill_broadcaster.send(True)
else:
self._kill_broadcaster.send(False)
def _on_float_push_btn_toggle(self, checked):
# TODO: Check if float service active
try:
if checked:
self._float_proxy(True)
else:
self._float_proxy(False)
except rospy.service.ServiceException:
pass
# Combo box callbacks
def _controller_combo_box_cb(self, text):
self._request_controller_proxy(text)
# Update functions
def _updateStatus(self):
# Check if killed
if self._kill_listener.get_killed():
self._kill_label.setPixmap(self._red_indicator)
else:
self._kill_label.setPixmap(self._green_indicator)
# Check float status
if self._float_status:
self._float_label.setPixmap(self._red_indicator)
else:
self._float_label.setPixmap(self._green_indicator)
# Check if in autonomous or RC
self._controller_label.setText(self._current_controller)
def _updateControl(self):
# Wait until we get the first controller
if self._controller == '':
return
controllers = self._get_all_controllers_proxy().controllers
if controllers != self._controllers:
self._controllers = controllers
self._controller_combo_box.clear()
self._controller_combo_box.addItems(controllers)
index = self._controller_combo_box.findText(self._controller)
if index != -1 and index != self._controller_combo_box.currentIndex():
self._controller_combo_box.setCurrentIndex(index)
def _updateLidar(self):
pass
def _onUpdate(self):
self._updateStatus()
self._updateControl()
self._updateLidar()
self._odom_update()
class KillPlugin(Plugin):
def __init__(self, context):
super(KillPlugin, self).__init__(context)
self.setObjectName('KillPlugin')
self._listener = KillListener()
self._broadcaster = KillBroadcaster(rospy.get_name(),
'Software kill using KillPlugin')
self._kill_active = False
self._widget = QWidget()
loadUi(os.path.join(uipath, 'killplugin.ui'), self._widget)
context.add_widget(self._widget)
self._widget.findChild(QTableWidget,
'killTable').setHorizontalHeaderLabels(
["Name", "Status", "Description"])
self._widget.findChild(QPushButton, 'killButton').clicked.connect(
self._on_kill_clicked)
self._widget.findChild(QPushButton, 'unkillButton').clicked.connect(
self._on_unkill_clicked)
self._widget.findChild(QPushButton, 'runButton').clicked.connect(
self._on_run_clicked)
self._update_timer = QTimer(self._widget)
self._update_timer.timeout.connect(self._on_update)
self._update_timer.start(100)
def _on_update(self):
self._update_kills()
self._update_kill()
def _on_kill_clicked(self):
self._kill_active = True
self._broadcaster.send(True)
self._update_kill()
def _on_unkill_clicked(self):
self._kill_active = False
self._broadcaster.send(False)
self._update_kill()
def _on_run_clicked(self):
self._on_unkill_clicked()
def _update_kills(self):
all_kills = self._listener.get_all_kills()
if all_kills is not None:
new_kills = {
kill.id: kill
for kill in self._listener.get_all_kills()
}
else:
new_kills = {}
table = self._widget.findChild(QTableWidget, 'killTable')
row = 0
while row < table.rowCount():
id = table.item(row, 1)
if id in new_kills:
self._update_kill_entry(row, new_kills[id])
del new_kills[id]
row += 1
else:
table.removeRow(row)
for kill in new_kills.values():
row = table.rowCount()
table.setRowCount(row + 1)
self._update_kill_entry(row, kill)
def _update_kill_entry(self, row, kill):
color = QColor(255, 255, 255) if not kill.active else QColor(
255, 200, 200)
self._update_item(row, 0, color, kill.id)
self._update_item(row, 1, color,
"Killed" if kill.active else "Unkilled")
self._update_item(row, 2, color, kill.description)
def _update_item(self, row, col, color, string):
item = QTableWidgetItem(string)
item.setBackground(color)
self._widget.findChild(QTableWidget,
'killTable').setItem(row, col, item)
def _update_kill(self):
kills = self._listener.get_all_kills()
if kills is not None:
other_kill_count = len([
kill for kill in kills
if kill.id != rospy.get_name() and kill.active
])
self._widget.findChild(
QPushButton, 'runButton').setVisible(other_kill_count == 0)
self._widget.findChild(
QPushButton, 'unkillButton').setVisible(other_kill_count > 0)
status = 'Sub status: '
if not self._listener.get_killed():
status += '<span style="color:green">Running</span>'
else:
status += '<span style="color:red">Killed</span>'
self._widget.findChild(QLabel, 'killStatusLabel').setText(status)
else:
self._widget.findChild(QPushButton, 'runButton').setVisible(False)
self._widget.findChild(QPushButton,
'unkillButton').setVisible(False)
self._widget.findChild(QLabel, 'killStatusLabel').setText(
'<span style="color:red">No kill information</span>')
class KillPlugin(Plugin):
def __init__(self, context):
super(KillPlugin, self).__init__(context)
self.setObjectName('KillPlugin')
self._listener = KillListener()
self._broadcaster = KillBroadcaster(rospy.get_name(), 'Software kill using KillPlugin')
self._kill_active = False
self._widget = QWidget()
loadUi(os.path.join(uipath, 'killplugin.ui'), self._widget)
context.add_widget(self._widget)
self._widget.findChild(QTableWidget, 'killTable').setHorizontalHeaderLabels(["Name", "Status", "Description"])
self._widget.findChild(QPushButton, 'killButton').clicked.connect(self._on_kill_clicked)
self._widget.findChild(QPushButton, 'unkillButton').clicked.connect(self._on_unkill_clicked)
self._widget.findChild(QPushButton, 'runButton').clicked.connect(self._on_run_clicked)
self._update_timer = QTimer(self._widget)
self._update_timer.timeout.connect(self._on_update)
self._update_timer.start(100)
def _on_update(self):
self._update_kills()
self._update_kill()
def _on_kill_clicked(self):
self._kill_active = True
self._broadcaster.send(True)
self._update_kill()
def _on_unkill_clicked(self):
self._kill_active = False
self._broadcaster.send(False)
self._update_kill()
def _on_run_clicked(self):
self._on_unkill_clicked()
def _update_kills(self):
all_kills = self._listener.get_all_kills()
if all_kills is not None:
new_kills = {kill.id: kill for kill in self._listener.get_all_kills()}
else:
new_kills = {}
table = self._widget.findChild(QTableWidget, 'killTable')
row = 0
while row < table.rowCount():
id = table.item(row, 1)
if id in new_kills:
self._update_kill_entry(row, new_kills[id])
del new_kills[id]
row += 1
else:
table.removeRow(row)
for kill in new_kills.values():
row = table.rowCount()
table.setRowCount(row+1)
self._update_kill_entry(row, kill)
def _update_kill_entry(self, row, kill):
color = QColor(255, 255, 255) if not kill.active else QColor(255, 200, 200)
self._update_item(row, 0, color, kill.id)
self._update_item(row, 1, color, "Killed" if kill.active else "Unkilled")
self._update_item(row, 2, color, kill.description)
def _update_item(self, row, col, color, string):
item = QTableWidgetItem(string)
item.setBackground(color)
self._widget.findChild(QTableWidget, 'killTable').setItem(row, col, item)
def _update_kill(self):
kills = self._listener.get_all_kills()
if kills is not None:
other_kill_count = len([kill for kill in kills
if kill.id != rospy.get_name() and kill.active])
self._widget.findChild(QPushButton, 'runButton').setVisible(other_kill_count == 0)
self._widget.findChild(QPushButton, 'unkillButton').setVisible(other_kill_count > 0)
status = 'Sub status: '
if not self._listener.get_killed():
status += '<span style="color:green">Running</span>'
else:
status += '<span style="color:red">Killed</span>'
self._widget.findChild(QLabel, 'killStatusLabel').setText(status)
else:
self._widget.findChild(QPushButton, 'runButton').setVisible(False)
self._widget.findChild(QPushButton, 'unkillButton').setVisible(False)
self._widget.findChild(QLabel, 'killStatusLabel').setText('<span style="color:red">No kill information</span>')
