L1.5/l1.5-safety-system
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

Added basic handling of obstacle detection in user plugin

Browse Source
This commit is contained in:
Olek Bojda 2018-09-29 15:38:34 +02:00
parent 6cd0ce3934
commit caa4b1a8fc
5 changed files with 41 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
rosaria_msgs/msg/RobotInfoMsg.msg
View File

@ -2,4 +2,5 @@ std_msgs/UInt8 robot_id
std_msgs/Float64 battery_voltage
geometry_msgs/Twist twist
std_msgs/Bool state
std_msgs/Bool clutch
std_msgs/Bool clutch
std_msgs/Bool obstacle_detected

10
safety_user_plugin/scripts/qt_wrapper.py
View File

@ -221,6 +221,9 @@ class QtWrapper:
def master_is_stopped_notify(self):
self.log_error('Nie mozna wystartowac robota. MasterSTOP wcisniety!')
def obstacle_is_detected_notify(self):
self.log_error('Nie mozna wystartowac. Przeszkoda w polu dzialania robota')
def update_restrictions_gui(self,restrictions):
self.widget.distanceRestrictionLabel.setText("{:.2f}".format(restrictions.distance))
self.widget.linearRestrictionLabel.setText("{:.2f}".format(restrictions.linear_velocity))
@ -267,6 +270,13 @@ class QtWrapper:
def connection_to_master_lost(self):
self.log_error('Utrata polaczenia z masterstopem. Popros prowadzacego o jego uruchomienie')
def obstacle_detected(self):
self.log_error('Przeszkoda wykryta. Zatrzymuje robota')
# TODO Zmiana
def obstacle_removed(self):
self.log_info('Przeszkoda usunieta')
def scroll_to_bottom(self):
scrollbar = self.widget.logsBrowser.verticalScrollBar()
scrollbar.setValue(scrollbar.maximum())

2
safety_user_plugin/scripts/robot_info.py
View File

@ -12,12 +12,14 @@ class RobotInfo:
self.angular_velocity = None
self.state = None
self.clutch = None
self.obstacle_detected = None
def update_robot_info(self,robot_info_msg):
self.robot_id = robot_info_msg.robot_id.data
self.battery_voltage = robot_info_msg.battery_voltage.data
self.linear_velocity = [robot_info_msg.twist.linear.x,robot_info_msg.twist.linear.y,robot_info_msg.twist.linear.z]
self.angular_velocity = [robot_info_msg.twist.angular.x,robot_info_msg.twist.angular.y,robot_info_msg.twist.angular.z]
self.obstacle_detected = robot_info_msg.obstacle_detected.data
if robot_info_msg.state.data == True:
self.state = SS.RUNNING

1
safety_user_plugin/scripts/user_info.py
View File

@ -27,6 +27,7 @@ class UserInfo:
self.selected_robot.angular_velocity = new_robot_info.angular_velocity
self.selected_robot.state = new_robot_info.state
self.selected_robot.clutch = new_robot_info.clutch
self.selected_robot.obstacle_detected = new_robot_info.obstacle_detected
self.clutch_state = new_robot_info.clutch
def get_user_stop_state(self):

36
safety_user_plugin/scripts/user_plugin.py
View File

@ -129,17 +129,20 @@ class UserPlugin(Plugin):
raise RuntimeError('Cannot release. No robot selected')
def handle_user_stop_state_updated(self):
if self._user_info.master_stop_state == SS.STOPPED:
self._qt_wrapper.master_is_stopped_notify()
elif self._user_info.master_stop_state == SS.RUNNING:
if self._user_info.user_stop_state == SS.RUNNING:
self.user_stopped()
elif self._user_info.user_stop_state == SS.STOPPED:
self.user_started()
else:
raise ValueError('user_stop_state value undefined')
if self._user_info.selected_robot.obstacle_detected == True:
self._qt_wrapper.obstacle_is_detected_notify()
else:
raise ValueError('master_stop_state value undefined')
if self._user_info.master_stop_state == SS.STOPPED:
self._qt_wrapper.master_is_stopped_notify()
elif self._user_info.master_stop_state == SS.RUNNING:
if self._user_info.user_stop_state == SS.RUNNING:
self.user_stopped()
elif self._user_info.user_stop_state == SS.STOPPED:
self.user_started()
else:
raise ValueError('user_stop_state value undefined')
else:
raise ValueError('master_stop_state value undefined')
def handle_clutch_switched(self):
if self._user_info.clutch_state == CS.ENGAGED:
@ -227,7 +230,20 @@ class UserPlugin(Plugin):
self.master_stopped()
self.call_qt_wrapper_method('connection_to_master_lost')
def obstacle_detected(self):
self.call_qt_wrapper_method('obstacle_detected')
self.user_stopped()
def obstacle_removed(self):
self.call_qt_wrapper_method('obstacle_removed')
def update_selected_robot_info(self,robot_info):
if self._user_info.select_robot != None:
if robot_info.obstacle_detected == True and self._user_info.selected_robot.obstacle_detected == False:
self.obstacle_detected()
elif robot_info.obstacle_detected == False and self._user_info.selected_robot.obstacle_detected == True:
self.obstacle_removed()
self._user_info.update_selected_robot_info(robot_info)
self.call_qt_wrapper_method_with_argument('update_selected_robot_info',[robot_info])
# self._qt_wrapper.update_selected_robot_info(robot_info)