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

Added functionality to check if robot is already selected by another group

Browse Source
This commit is contained in:
Olek Bojda 2018-10-22 19:50:03 +02:00
parent 511d552eff
commit 504000d496
3 changed files with 42 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
safety_user_plugin/scripts/qt_wrapper.py
View File

@ -236,6 +236,9 @@ class QtWrapper:
def obstacle_is_detected_notify(self):
self.log_error('Nie mozna wystartować. Przeszkoda w polu działania robota')
def robot_selected_by_another_group_notify(self):
self.log_error('Nie mozna wybrać robota. Robot został już wybrany przez inną grupę')
def update_restrictions_gui(self,restrictions):
self.widget.distanceRestrictionLabel.setText("{:.2f}".format(restrictions.distance))
self.widget.linearRestrictionLabel.setText("{:.2f}".format(restrictions.linear_velocity))

35
safety_user_plugin/scripts/ros_wrapper.py
View File

@ -1,6 +1,7 @@
#!/usr/bin/env python
import rospy
import rospkg
import rostopic
from std_msgs.msg import Bool
from rosaria_msgs.msg import RobotInfoMsg
@ -34,6 +35,11 @@ class ROSWrapper:
self.robots_list_timer = None
self.connection_timer = None
self.master_connection_timer = None
self.hz_update_timer = rospy.Timer(rospy.Duration(0.2),self.update_hz_values)
self.robots_hz_subscribers_dict = {}
self.rostopics_hz_dict = {}
self.robots_hz_value = {}
# def setup_node(self):
# rospy.init_node('safety_user_plugin', anonymous=True)
@ -50,7 +56,7 @@ class ROSWrapper:
if self.periodic_timer != None:
self.shutdown_timer(self.periodic_timer)
self.periodic_timer = rospy.Timer(rospy.Duration(0.2),self.publish_periodic_update)
self.periodic_timer = rospy.Timer(rospy.Duration(0.1),self.publish_periodic_update)
if self.connection_timer != None:
self.shutdown_timer(self.connection_timer)
@ -60,6 +66,10 @@ class ROSWrapper:
self.shutdown_timer(self.master_connection_timer)
self.master_connection_timer = rospy.Timer(rospy.Duration(1.5),self.handle_master_connection_lost)
if self.hz_update_timer != None:
self.shutdown_timer(self.hz_update_timer)
self.hz_update_timer = rospy.Timer(rospy.Duration(0.2),self.update_hz_values)
def unsubscribe(self,subscriber):
if subscriber != None:
subscriber.unregister()
@ -90,6 +100,11 @@ class ROSWrapper:
# else:
# raise ValueError('clutch_state UNKNOWN when attempting to publish periodic update')
def update_hz_values(self,event):
for key in self.rostopics_hz_dict:
self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz('/PIONIER{0}/RosAria/user_stop'.format(key))
def handle_robot_connection_lost(self,event):
self.shutdown_timer(self.connection_timer)
self.robot_connection_lost_callback()
@ -161,6 +176,24 @@ class ROSWrapper:
robot_number = int(robot_number)
robots_id_list.append(robot_number)
# Adding new hz subscribers
for robot_id in robots_id_list:
if robot_id not in self.robots_hz_subscribers_dict:
# Add hz subscriber
self.rostopics_hz_dict[robot_id] = rostopic.ROSTopicHz(-1)
self.robots_hz_subscribers_dict[robot_id] = rospy.Subscriber('/PIONIER{0}/RosAria/user_stop'.format(robot_id),Bool,
self.rostopics_hz_dict[robot_id].callback_hz,callback_args='/PIONIER{0}/RosAria/user_stop'.format(robot_id))
# Removing old hz subscribers
for robot_key in self.robots_hz_subscribers_dict:
if robot_key not in robots_id_list:
# Remove old subscriber
self.unsubscribe(self.robots_hz_subscribers_dict[robot_id])
self.robots_hz_subscribers_dict[robot_id] = None
self.rostopics_hz_dict[robot_id] = None
self.robots_list_update_callback(robots_id_list)

6
safety_user_plugin/scripts/user_plugin.py
View File

@ -118,7 +118,11 @@ class UserPlugin(Plugin):
# Qt callback functions
def handle_robot_selection(self,robot_id):
if self._user_info.selected_robot == None:
self.select_robot(robot_id)
if self._ros_wrapper.robots_hz_value[robot_id] == None:
self.select_robot(robot_id)
else:
self._qt_wrapper.robot_selected_by_another_group_notify()
else:
raise RuntimeError('User already selected robot')