def __init__(self, widget_class, name, window, *args):
super(OCSPlugin, self).__init__(None)
if len(args) > 0:
self._widget = widget_class(self, args[0])
else:
self._widget = widget_class(self)
self._name = name
self._window = window
self._window_control_sub = rospy.Subscriber("/flor/ocs/window_control",
Int8,
self.processWindowControl)
self._window_control_pub = rospy.Publisher("/flor/ocs/window_control",
Int8,
queue_size=10)
# window visibility configuration variables
self._last_window_control_data = -self._window
self._set_window_visibility = True
# this is only used to make sure we close window if ros::shutdown has already been called
self._timer = QBasicTimer()
self._timer.start(33, self)
settings = QSettings("OCS", self._name)
if settings.value("mainWindowGeometry") is not None:
self.restoreGeometry(settings.value("mainWindowGeometry"))
self.geometry_ = self.geometry()
class JointControlDialog(QWidget):
def __init__(self):
super(JointControlDialog, self).__init__(None)
self._widget = JointControlWidget(self)
self._window_control_sub = rospy.Subscriber("/flor/ocs/window_control", Int8, self.processWindowControl)
self._window_control_pub = rospy.Publisher("/flor/ocs/window_control", Int8, queue_size=10)
# #define WINDOW_JOINT_CONTROL 7
self._window = 7
# window visibility configuration variables
self._last_window_control_data = -self._window
self._set_window_visibility = True
# this is only used to make sure we close window if ros::shutdown has already been called
self._timer = QBasicTimer()
self._timer.start(33, self)
settings = QSettings("OCS", "joint_control")
if settings.value("mainWindowGeometry") is not None:
self.restoreGeometry(settings.value("mainWindowGeometry"))
self.geometry_ = self.geometry()
def processWindowControl(self, visible):
# set window visibility changed flag
self._set_window_visibility = True
self._last_window_control_data = visible.data
def timerEvent(self, event):
if rospy.is_shutdown():
QCoreApplication.instance().quit()
# needed to move qt calls out of the ros callback, otherwise qt crashes because of inter-thread communication
if self._set_window_visibility:
self._set_window_visibility = False
if not self.isVisible() and self._last_window_control_data == self._window:
self.show()
self.setGeometry(self._geometry)
elif self.isVisible() and (not self._last_window_control_data or self._last_window_control_data == -self._window):
self._geometry = self.geometry()
self.hide()
def closeEvent(self, event):
settings = QSettings("OCS", "joint_control")
settings.setValue("mainWindowGeometry", self.saveGeometry())
msg = Int8()
msg.data = -self._window
self._window_control_pub.publish(msg)
event.ignore()
def resizeEvent(self, event):
settings = QSettings("OCS", "joint_control")
settings.setValue("mainWindowGeometry", self.saveGeometry())
def moveEvent(self, event):
settings = QSettings("OCS", "joint_control")
settings.setValue("mainWindowGeometry", self.saveGeometry())
def _init_timers(self): ''' inits the timers used to control the connection, .. ''' # Creates a basic timer and starts it self._timer = QBasicTimer() self._timer.start(self._update_timer_interface, self)
def __init__(self, widget_class, name, window, *args):
super(OCSPlugin, self).__init__(None)
if len(args) > 0:
self._widget = widget_class(self, args[0])
else:
self._widget = widget_class(self)
self._name = name
self._window = window
self._window_control_sub = rospy.Subscriber("/flor/ocs/window_control", Int8, self.processWindowControl)
self._window_control_pub = rospy.Publisher("/flor/ocs/window_control", Int8, queue_size=10)
# window visibility configuration variables
self._last_window_control_data = -self._window
self._set_window_visibility = True
# this is only used to make sure we close window if ros::shutdown has already been called
self._timer = QBasicTimer()
self._timer.start(33, self)
settings = QSettings("OCS", self._name)
if settings.value("mainWindowGeometry") != None:
self.restoreGeometry(settings.value("mainWindowGeometry"))
self.geometry_ = self.geometry()
class ControlModeDialog(QWidget):
def __init__(self):
super(ControlModeDialog, self).__init__(None)
self._control_mode_widget = ControlModeWidget(self)
self._window_control_sub = rospy.Subscriber("/flor/ocs/window_control", Int8, self.processWindowControl)
# #define WINDOW_CONTROL_MODE 9
self._window = 9
# window visibility configuration variables
self._last_window_control_data = -self._window
self._set_window_visibility = True
# this is only used to make sure we close window if ros::shutdown has already been called
self._timer = QBasicTimer()
self._timer.start(33, self)
def processWindowControl(self, visible):
# set window visibility changed flag
self._set_window_visibility = True
self._last_window_control_data = visible.data
def timerEvent(self, event):
if rospy.is_shutdown():
QCoreApplication.instance().quit();
# needed to move qt calls out of the ros callback, otherwise qt crashes because of inter-thread communication
if self._set_window_visibility:
self._set_window_visibility = False
if not self.isVisible() and self._last_window_control_data == self._window:
self.show()
self.setGeometry(self._geometry)
elif self.isVisible() and (not self._last_window_control_data or self._last_window_control_data == -self._window):
self._geometry = self.geometry()
self.hide()
def __init__(self):
super(ControlModeDialog, self).__init__(None)
self._control_mode_widget = ControlModeWidget(self)
self._window_control_sub = rospy.Subscriber("/flor/ocs/window_control", Int8, self.processWindowControl)
# #define WINDOW_CONTROL_MODE 9
self._window = 9
# window visibility configuration variables
self._last_window_control_data = -self._window
self._set_window_visibility = True
# this is only used to make sure we close window if ros::shutdown has already been called
self._timer = QBasicTimer()
self._timer.start(33, self)
def __init__(self):
super(JointControlDialog, self).__init__(None)
self._widget = JointControlWidget(self)
self._window_control_sub = rospy.Subscriber("/flor/ocs/window_control", Int8, self.processWindowControl)
self._window_control_pub = rospy.Publisher("/flor/ocs/window_control", Int8, queue_size=10)
# #define WINDOW_JOINT_CONTROL 7
self._window = 7
# window visibility configuration variables
self._last_window_control_data = -self._window
self._set_window_visibility = True
# this is only used to make sure we close window if ros::shutdown has already been called
self._timer = QBasicTimer()
self._timer.start(33, self)
settings = QSettings("OCS", "joint_control")
if settings.value("mainWindowGeometry") is not None:
self.restoreGeometry(settings.value("mainWindowGeometry"))
self.geometry_ = self.geometry()
class OCSPlugin(QWidget):
def __init__(self, widget_class, name, window, *args):
super(OCSPlugin, self).__init__(None)
if len(args) > 0:
self._widget = widget_class(self, args[0])
else:
self._widget = widget_class(self)
self._name = name
self._window = window
self._window_control_sub = rospy.Subscriber("/flor/ocs/window_control",
Int8,
self.processWindowControl)
self._window_control_pub = rospy.Publisher("/flor/ocs/window_control",
Int8,
queue_size=10)
# window visibility configuration variables
self._last_window_control_data = -self._window
self._set_window_visibility = True
# this is only used to make sure we close window if ros::shutdown has already been called
self._timer = QBasicTimer()
self._timer.start(33, self)
settings = QSettings("OCS", self._name)
if settings.value("mainWindowGeometry") is not None:
self.restoreGeometry(settings.value("mainWindowGeometry"))
self.geometry_ = self.geometry()
def processWindowControl(self, visible):
# set window visibility changed flag
self._set_window_visibility = True
self._last_window_control_data = visible.data
def timerEvent(self, event):
if rospy.is_shutdown():
QCoreApplication.instance().quit()
# needed to move qt calls out of the ros callback, otherwise qt crashes because of inter-thread communication
if self._set_window_visibility:
self._set_window_visibility = False
if not self.isVisible(
) and self._last_window_control_data == self._window:
self.show()
self.setGeometry(self._geometry)
elif self.isVisible() and (not self._last_window_control_data
or self._last_window_control_data
== -self._window):
self._geometry = self.geometry()
self.hide()
def closeEvent(self, event):
settings = QSettings("OCS", self._name)
settings.setValue("mainWindowGeometry", self.saveGeometry())
msg = Int8()
msg.data = -self._window
self._window_control_pub.publish(msg)
event.ignore()
def resizeEvent(self, event):
settings = QSettings("OCS", self._name)
settings.setValue("mainWindowGeometry", self.saveGeometry())
def moveEvent(self, event):
settings = QSettings("OCS", self._name)
settings.setValue("mainWindowGeometry", self.saveGeometry())
class MissionCommanderGUI(Plugin):
def __init__(self, context):
super(MissionCommanderGUI, self).__init__(context)
self.setObjectName('MissionCommanderGUI')
self._widget = QWidget()
rp = rospkg.RosPack()
self.state_string = " "
# UI
ui_file = os.path.join(rp.get_path('robospect_mission_commander'), 'resource', 'MissionCommander_v1.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('MissionCommanderGUI')
pixmap_red_file = os.path.join(rp.get_path('robospect_mission_commander'), 'resource', 'red.png')
pixmap_green_file = os.path.join(rp.get_path('robospect_mission_commander'), 'resource', 'green.png')
pixmap_orange_file = os.path.join(rp.get_path('robospect_mission_commander'), 'resource', 'orange.png')
self._pixmap_red = QPixmap(pixmap_red_file)
self._pixmap_green = QPixmap(pixmap_green_file)
self._pixmap_orange = QPixmap(pixmap_orange_file)
self._widget.label_platform_controller_state.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_platform_navigation_planner_state.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_platform_localization_state.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_platform_trajectory_planner_state.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_mission_state.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_pad_state.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_pad_vehicle_control.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_pad_crane_control.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_pan_motor_moving.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_switches_state.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_sw1_state.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_sw2_state.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_sw3_state.setPixmap(self._pixmap_red) # Shows connection state
self._widget.label_sw4_state.setPixmap(self._pixmap_red) # Shows connection state
for command in mission_commands:
self._widget.comboBox_command.addItem(command)
for mode in control_modes:
self._widget.comboBox_control_mode.addItem(mode)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Adds this widget to the context
context.add_widget(self._widget)
self._mission_state_topic_connected = False
# Inits connections and gui components values
self._init()
# HANDLERS
# Adds handlers to 'press button' event
self._widget.pushButton_initialize_platform.clicked.connect(self._initialize_platform_cb)
self._widget.pushButton_reset_steering_encoder.clicked.connect(self._reset_steering_encoder_cb)
self._widget.pushButton_send.clicked.connect(self._send_comand_cb)
self._widget.pushButton_set_control_mode.clicked.connect(self._set_control_mode_cb)
self._widget.pushButton_fold_crane.clicked.connect(self._fold_crane_cb)
self._widget.pushButton_stop_vehicle_action.clicked.connect(self._stop_vehicle_cb)
self._widget.pushButton_set_pantilt.clicked.connect(self._set_pantilt_cb)
self._widget.pushButton_stop_teleop.clicked.connect(self._stop_teleop_cb)
self._update_timer_interface = 100 # Frequency to update values in the interface (ms)
self.waypoints_markers = PointPathManager('mission_waypoints', frame_id = '/map')
self._init_timers()
def _init(self):
'''
Inits topic names, topic and service connections
'''
self._command_service = rospy.get_param('/services/mission_command', default='/mission_command_srv')
self._init_platform_service = rospy.get_param('/services/initialize_platform', default='/robospect_platform_controller/initialize_modbus_controller')
self._reset_steering_encoder_service = rospy.get_param('/services/reset_steering_encoder', default='/robospect_platform_controller/reset_steering_encoder')
self._set_control_mode_service = rospy.get_param('/services/set_control_mode', default='/robospect_platform_controller/set_control_mode')
self._platform_service = rospy.get_param('/services/platform_command', default='/platform_command_srv')
self._mission_state_topic = rospy.get_param('/topics/mission_state', default='/mission_state')
self._platform_controller_state_topic = rospy.get_param('/topics/platform_controller_state', default='/robospect_platform_controller/state')
self._navigation_planner_state_topic = rospy.get_param('/topics/navigation_planner_state', default='/robospect_planner/state')
self._localization_state_topic = rospy.get_param('/topics/localization_state', default='/nav200_laser_node/nav200_pose')
self._trajectory_planner_state_topic = rospy.get_param('/topics/trajectory_planner_state', default='/rt_traj_planner/state')
self._trajectory_control_state_topic = rospy.get_param('/topics/trajectory_control_state', default='/rt_traj_exec/state')
self._pan_state_topic = rospy.get_param('/topics/pan_state_topic', default='/pan_controller/state')
self._tilt_state_topic = rospy.get_param('/topics/tilt_state_topic', default='/tilt_controller/state')
self._pad_state_topic = rospy.get_param('/topics/pad_state_topic', default='/robospect_pad/state')
self._teleop_done_topic = rospy.get_param('/topics/teleop_done', default='/teleop_done')
self._contact_switches_topic = rospy.get_param('/topics/contact_switches_topic', default='/contact_switches_state')
#self._map_frame_id = = rospy.get_param('/global_params/map_frame_id', default='/map')
#self._base_frame_id = = rospy.get_param('/global_params/base_frame_id', default='/base_footprint')
# Attribute to save the current mission state
self._components_state = {}
self._components_state['mission_state'] = {'state': MissionState(), 'time': rospy.Time.now(), 'label': self._widget.label_mission_state}
self._components_state['platform_controller_state'] = {'state': RobospectState(), 'time': rospy.Time.now(), 'label': self._widget.label_platform_controller_state}
self._components_state['navigation_planner_state'] = {'state': State(), 'time': rospy.Time.now(), 'label': self._widget.label_platform_navigation_planner_state}
self._components_state['localization_state'] = {'state': Pose2D_nav(), 'time': rospy.Time.now(), 'label': self._widget.label_platform_localization_state}
self._components_state['trajectory_planner_state'] = {'state': State(), 'time': rospy.Time.now(), 'label': self._widget.label_platform_trajectory_planner_state}
self._components_state['pan_state'] = {'state': DynamixelState(), 'time': rospy.Time.now(), 'label': self._widget.label_pan_state, 'label_moving': self._widget.label_pan_motor_moving}
self._components_state['tilt_state'] = {'state': DynamixelState(), 'time': rospy.Time.now(), 'label': self._widget.label_tilt_state, 'label_moving': self._widget.label_tilt_motor_moving}
self._components_state['trajectory_control_state'] = {'state': State(), 'time': rospy.Time.now(), 'label': None}
self._components_state['pad_state'] = {'state': PadStatus(), 'time': rospy.Time.now(), 'label': self._widget.label_pad_state, 'label_vehicle': self._widget.label_pad_vehicle_control, 'label_crane': self._widget.label_pad_crane_control}
self._components_state['contact_switches_state'] = {'state': Quaternion(), 'time': rospy.Time.now(), 'label': self._widget.label_switches_state, 'label_sw1': self._widget.label_sw1_state, 'label_sw2': self._widget.label_sw2_state,'label_sw3': self._widget.label_sw3_state,'label_sw4': self._widget.label_sw4_state, }
# SUBSCRIPTIONS
self._mission_state_sub = rospy.Subscriber(self._mission_state_topic, MissionState, self._mission_state_cb, queue_size = 10)
self._platform_controller_state_sub = rospy.Subscriber(self._platform_controller_state_topic, RobospectState, self._platform_controller_state_cb, queue_size=10)
self._navigation_planner_state_sub = rospy.Subscriber(self._navigation_planner_state_topic, PlannerState, self._navigation_planner_state_cb, queue_size=10)
self._localization_state_sub = rospy.Subscriber(self._localization_state_topic, Pose2D_nav, self._localization_state_cb, queue_size=10)
self._trajectory_planner_state_sub = rospy.Subscriber(self._trajectory_planner_state_topic, TrajectoryPlannerState, self._trajectory_planner_state_cb, queue_size=10)
self._trajectory_control_state_sub = rospy.Subscriber(self._trajectory_control_state_topic, State, self._trajectory_control_state_cb, queue_size=10)
self._pan_state_sub = rospy.Subscriber(self._pan_state_topic, DynamixelState, self._pan_state_cb, queue_size = 2)
self._tilt_state_sub = rospy.Subscriber(self._tilt_state_topic, DynamixelState, self._tilt_state_cb, queue_size = 2)
self._pad_state_sub = rospy.Subscriber(self._pad_state_topic, PadStatus, self._pad_state_cb, queue_size = 2)
self._contact_switch_state_sub = rospy.Subscriber(self._contact_switches_topic, Quaternion, self._contact_switches_state_cb, queue_size = 2)
# transform listener to look for transformations
#self._transform_listener = TransformListener()
# PUBLICATIONS
self.teleop_done_pub = rospy.Publisher(self._teleop_done_topic, Bool, queue_size=10)
'''
if hasattr(self, ''):
self..unregister()
rospy.loginfo('Unregistering from command publisher')
try:
self. = rospy.Publisher(self._command_service, MissionCommand, queue_size=10)
except ROSException, e:
rospy.logerr('MissionCommanderGUI: Error creating publisher for topic %s (%s)'%(self._command_service, e))
'''
# Services
self._command_service_client = rospy.ServiceProxy(self._command_service, MissionCommandSrv)
self._initialize_platform_service_client = rospy.ServiceProxy(self._init_platform_service, Empty)
self._reset_steering_encoder_service_client = rospy.ServiceProxy(self._reset_steering_encoder_service, Empty)
self._set_control_mode_service_client = rospy.ServiceProxy(self._set_control_mode_service, SetControlMode)
self._platform_command_service_client = rospy.ServiceProxy(self._platform_service, PlatformCommandSrv)
def _init_timers(self):
'''
inits the timers used to control the connection, ..
'''
# Creates a basic timer and starts it
self._timer = QBasicTimer()
self._timer.start(self._update_timer_interface, self)
def _send_comand_cb(self):
'''
Sends a mission command
'''
msg = MissionCommand()
msg.command = self._widget.comboBox_command.currentText()
try:
msg.variable = float(self._widget.lineEdit_parameter.text().encode("utf8"))
except ValueError, e:
rospy.logerr('MissionCommanderGUI:_send_command_cb: %s',e)
QMessageBox.warning(self._widget, 'Error', 'Incorrect format of the parameter. A number is expected')
srv = MissionCommandSrv()
if msg.command == 'complete' or msg.command == 'complete_offline':
waypoints = self.waypoints_markers.getMissionPoints()
if len(waypoints) == 0:
QMessageBox.warning(self._widget, 'Error', 'No waypoints to send')
return
msg.points = waypoints
#srv.command = msg
try:
self._command_service_client(msg)
except rospy.ROSInterruptException,e:
rospy.logerr('MissionCommanderGUI:_send_command_cb: %s',e)
QMessageBox.warning(self._widget, 'Error', 'Error sending the command')
