diff --git a/safety_master_plugin/scripts/master_plugin.py b/safety_master_plugin/scripts/master_plugin.py index 3a0f01a..44f0ca5 100755 --- a/safety_master_plugin/scripts/master_plugin.py +++ b/safety_master_plugin/scripts/master_plugin.py @@ -16,15 +16,15 @@ class MasterPlugin(Plugin): self.setObjectName('Master Plugin - L1.5 safety system') self._widget = MasterWidget(context) + self._ros_wrapper = ROSWrapper() self._qt_wrapper = QtWrapper(self._widget) - self._master_info = MasterInfo() # self._qt_wrapper.connect_signals() # self._qt_wrapper.set_sliders_to_initial_values() - self._ros_wrapper = ROSWrapper() + # Setup functions to get _master_info states from _ros_wrapper self._ros_wrapper.setup_get_master_stop_state_function(self._master_info.get_master_stop_state) self._ros_wrapper.setup_get_restrictions_function(self._master_info.get_restrictions) diff --git a/safety_master_plugin/scripts/master_restrictions.py b/safety_master_plugin/scripts/master_restrictions.py new file mode 100644 index 0000000..5df47c8 --- /dev/null +++ b/safety_master_plugin/scripts/master_restrictions.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +class Restrictions: + + def __init__(self,distance,linear_velocity,angular_velocity): + self.distance = distance + self.linear_velocity = linear_velocity + self.angular_velocity = angular_velocity + + + def update_restrictions(self,restrictions_msg): + self.distance = restrictions_msg.distance.data + self.linear_velocity = restrictions_msg.linear_velocity.data + self.angular_velocity = restrictions_msg.angular_velocity.data \ No newline at end of file diff --git a/safety_master_plugin/scripts/qt_master_wrapper.py b/safety_master_plugin/scripts/qt_master_wrapper.py index 118bcc0..26b7173 100644 --- a/safety_master_plugin/scripts/qt_master_wrapper.py +++ b/safety_master_plugin/scripts/qt_master_wrapper.py @@ -9,6 +9,8 @@ import datetime from enums.clutch_state import ClutchState as CS from enums.stop_state import StopState as SS +from master_restrictions import Restrictions + from python_qt_binding.QtGui import QPixmap from python_qt_binding.QtGui import QTextCursor from python_qt_binding.QtWidgets import QMessageBox @@ -20,15 +22,22 @@ class QtWrapper: self.widget = widget self.displayed_robots_id_list = [] - self.disable_sliders_tracking() - # self.set_sliders_to_initial_values() - self.master_stop_state = None + + # self.disable_sliders_tracking() + # self.ok_pixmap = QPixmap('/home/olek/safetysystemL1.5/src/SafetySystem/safety_user_plugin/ui/Ok.jpg') # self.cancel_pixmap = QPixmap('/home/olek/safetysystemL1.5/src/SafetySystem/safety_user_plugin/ui/Cancel.jpg') def set_sliders_to_initial_values(self): - raise NotImplementedError + self.widget.distanceSlider.setMaximum(200) + self.widget.angularSlider.setMaximum(200) + self.widget.linearSlider.setMaximum(200) + + self.widget.distanceSlider.setValue(50) + self.widget.angularSlider.setValue(100) + self.widget.linearSlider.setValue(100) + def disable_sliders_tracking(self): self.widget.distanceSlider.setTracking(False) @@ -43,18 +52,27 @@ class QtWrapper: self.restrictions_updated_callback = callback_function def handle_restrictions_update(self): - raise NotImplementedError - # restrictions = Restrictions() - # Do something - # self.restrictions_updated_callback(restrictions) + distance = (float(self.widget.distanceSlider.value())/100) + angular_velocity = (float(self.widget.angularSlider.value())/100) + linear_velocity = (float(self.widget.linearSlider.value())/100) + + self.widget.distanceLabel.setText("{:.2f}".format(distance)) + self.widget.angularLabel.setText("{:.2f}".format(angular_velocity)) + self.widget.linearLabel.setText("{:.2f}".format(linear_velocity)) + + restrictions = Restrictions(distance,angular_velocity,linear_velocity) + + self.restrictions_updated_callback(restrictions) def connect_signals(self): - # self.widget.distanceSlider.valueChanged.connect(self.handle_restrictions_update) - # self.widget.angularSlider.valueChanged.connect(self.handle_restrictions_update) - # self.widget.linearSlider.valueChanged.connect(self.handle_restrictions_update) + self.widget.distanceSlider.valueChanged.connect(self.handle_restrictions_update) + self.widget.angularSlider.valueChanged.connect(self.handle_restrictions_update) + self.widget.linearSlider.valueChanged.connect(self.handle_restrictions_update) self.widget.masterstopButton.clicked.connect(self.handle_masterstopButton_clicked) + self.set_sliders_to_initial_values() + # def handle_emitted_signal(self,method_name): # exec('self.{0}()'.format(method_name)) diff --git a/safety_master_plugin/scripts/restrictions.py b/safety_master_plugin/scripts/restrictions.py deleted file mode 100644 index bea4082..0000000 --- a/safety_master_plugin/scripts/restrictions.py +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -class Restrictions: - - def __init__(self,restrictions_msg): - self.distance = restrictions_msg.distance.data - self.linear_velocity = restrictions_msg.linear_velocity.data - self.angular_velocity = restrictions_msg.angular_velocity.data \ No newline at end of file diff --git a/safety_master_plugin/scripts/ros_master_wrapper.py b/safety_master_plugin/scripts/ros_master_wrapper.py index 77625ad..0e4343a 100644 --- a/safety_master_plugin/scripts/ros_master_wrapper.py +++ b/safety_master_plugin/scripts/ros_master_wrapper.py @@ -32,7 +32,7 @@ class ROSWrapper: self.master_stop_publisher = rospy.Publisher('/RosAria/master_stop',Bool,queue_size = 1) self.restrictions_publisher = rospy.Publisher('/RosAria/restrictions',RestrictionsMsg,queue_size = 1) - # self.periodic_timer = rospy.Timer(rospy.Duration(0.5),self.publish_periodic_update) + self.periodic_timer = rospy.Timer(rospy.Duration(0.5),self.publish_periodic_update) def add_robot_subscriber(self,robot_id): if robot_id not in self.robot_info_subscribers: