L1.5/safety_user_plugin
L1.5/safety_user_plugin
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

Implemented can't get robot:

Browse Source 
Signed-off-by: Jakub Delicat <jakub.delicat@pwr.edu.pl>
This commit is contained in:
Jakub Delicat 2023-09-13 12:16:11 +02:00
parent 9f6cfc2e9e
commit 11d64cb0cf
1 changed files with 35 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
safety_user_plugin/ros_wrapper.py
View File

@ -23,7 +23,6 @@ from safety_user_plugin.enums.stop_state import StopState as SS
class ROSWrapper(Node):
def __init__(self):
# TODO: mogą być problemy z tymi samymi nazwami node'ów, gry różne kompy będą miały odpalony
# safety user system
super().__init__("safety_user_plugin")
self.robots_list_update_callback = None
self.selected_robot_info_update_callback = None
@ -47,11 +46,18 @@ class ROSWrapper(Node):
self.connection_timer = None
self.master_connection_timer = None
self.robot_stop_timer = None
# self.hz_update_timer = self.create_timer(0.2, self.publish_user_stop_and_update_hz_values)
self.robots_hz_subscribers_dict = {}
self.rostopics_hz_dict = {}
self.robots_hz_value = {}
self.time_of_last_msg_of_user_stop = [
None for i in range(6)
]
self.user_stop_subscribers = [
None for i in range(6)
]
self.robots_hz_value = [
None for i in range(6)
]
self.hz_update_timer = self.create_timer(0.1, self.update_taken_robots)
self._qos = QoSProfile(
reliability=QoSReliabilityPolicy.RELIABLE,
@ -60,6 +66,20 @@ class ROSWrapper(Node):
liveliness=QoSLivelinessPolicy.AUTOMATIC,
depth=10,
)
self._create_user_stops_subscribers_and_callbacks()
def _create_user_stops_subscribers_and_callbacks(self):
for i in range(6):
self.user_stop_subscribers[i] = self.create_subscription(
Bool,
f"/pioneer{i}/user_stop",
lambda msg, i_in = i: self._update_user_stop_period(msg, i_in),
qos_profile=self._qos,
)
def _update_user_stop_period(self, msg, robot_id):
print(f"Got user stop message for {robot_id}: {msg.data}")
self.time_of_last_msg_of_user_stop[robot_id] = self.get_clock().now()
def setup_subscribers_and_publishers(self, robot_id):
self.robot_info_subscriber = self.create_subscription(
@ -96,16 +116,13 @@ class ROSWrapper(Node):
self.shutdown_timer(self.master_connection_timer)
self.master_connection_timer = self.create_timer(
1.5, self.handle_master_connection_lost()
1.5, self.handle_master_connection_lost
)
self.shutdown_timer(self.robot_stop_timer)
self.robot_stop_timer = self.create_timer(
0.1, self.publish_robot_stop
)
# 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 is not None:
@ -128,15 +145,16 @@ class ROSWrapper(Node):
msg.data = self.user_stop_state_to_publish
self.user_stop_publisher.publish(msg)
def publish_user_stop_and_update_hz_values(self):
print("update_hz_values")
def update_taken_robots(self):
for i, last_time in zip(range(6), self.time_of_last_msg_of_user_stop):
if last_time is None:
self.robots_hz_value[i] = None
continue
# for key in self.rostopics_hz_dict:
# self.robots_hz_value[key] = self.rostopics_hz_dict[key].get_hz(
# "/PIONEER{0}/RosAria/user_stop".format(key)
# )
self.robots_hz_value[i] = 1.0/float((self.get_clock().now() - last_time).nanoseconds) * 10e9
if self.robots_hz_value[i] < 20.0:
self.robots_hz_value[i] = None
print(f"hz values: {self.robots_hz_value}")
def handle_robot_connection_lost(self):
self.shutdown_timer(self.connection_timer)