cgras_dev_scripts/robot-reset-recovery-node.py at main · qcr/cgras_dev_scripts · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
#!/usr/bin/env python3

import rospy
from ur_dashboard_msgs.msg import RobotMode, SafetyMode
from ur_msgs.srv import SetIO, SetIORequest
from std_srvs.srv import Trigger, TriggerRequest
from std_srvs.srv import Empty, EmptyRequest
import time

class RobotModeMonitor:
    def __init__(self):
        rospy.init_node('robot_mode_monitor')

        self.last_mode = 5
        self.last_safety = 1

        # Subscriber to robot_mode topic
        self.sub_mode = rospy.Subscriber(
            '/ur_hardware_interface/robot_mode',
            RobotMode,
            self.robot_mode_callback
        )
        # Subscriber to safety_mode topic
        self.sub_safe = rospy.Subscriber(
            '/ur_hardware_interface/safety_mode',
            SafetyMode,
            self.robot_safe_callback
        )

        # Wait for the service to be available
        rospy.wait_for_service('/ur_hardware_interface/dashboard/quit')
        self.quit_srv = rospy.ServiceProxy(
            '/ur_hardware_interface/dashboard/quit',
            Trigger
        )
        rospy.wait_for_service('/ur_hardware_interface/dashboard/connect')
        self.connect_srv = rospy.ServiceProxy(
            '/ur_hardware_interface/dashboard/connect',
            Trigger
        )
        rospy.wait_for_service('/ur_hardware_interface/dashboard/close_safety_popup')
        self.close_safety_popup_srv = rospy.ServiceProxy(
            '/ur_hardware_interface/dashboard/close_safety_popup',
            Trigger
        )
        rospy.wait_for_service('/ur_hardware_interface/dashboard/stop')
        self.stop_srv = rospy.ServiceProxy(
            '/ur_hardware_interface/dashboard/stop',
            Trigger
        )
        rospy.wait_for_service('/ur_hardware_interface/dashboard/play')
        self.play_srv = rospy.ServiceProxy(
            '/ur_hardware_interface/dashboard/play',
            Trigger
        )
        rospy.wait_for_service('/ur_hardware_interface/set_io')
        self.make_it_green_srv = rospy.ServiceProxy(
            '/ur_hardware_interface/set_io',
            SetIO
        )
        rospy.loginfo("Service is available. Node ready.")

    def robot_safe_callback(self, msg):
        mode = msg.mode
        rospy.loginfo(f"Received safety mode: {mode}")

        if mode != self.last_safety:
            self.last_safety = mode
            if self.last_safety in [1]:
                self.call_safety_mode_service()

    def robot_mode_callback(self, msg):
        mode = msg.mode
        rospy.loginfo(f"Received robot mode: {mode}")

        if mode != self.last_mode:
            self.last_mode = mode
            if self.last_mode in [7]:
                self.call_safety_mode_service()

    def call_safety_mode_service(self):
        if self.last_mode in [7] and self.last_safety in [1]:
            rospy.loginfo("Calling get_safety_mode service...")
            try:
                response = self.quit_srv(TriggerRequest())
                time.sleep(5)
                print(f'Response: {response}')
                response = self.connect_srv(TriggerRequest())
                time.sleep(5)
                print(f'Response: {response}')
                response = self.close_safety_popup_srv(TriggerRequest())
                time.sleep(5)
                print(f'Response: {response}')
                response = self.stop_srv(TriggerRequest())
                time.sleep(5)
                print(f'Response: {response}')
                response = self.play_srv(TriggerRequest())
                time.sleep(5)
                print(f'Response: {response}')
                req = SetIORequest()
                req.fun = 1
                req.pin = 4
                req.state = 1.0
                response = self.make_it_green_srv(req)
                time.sleep(5)
                print(f'Response: {response}')

            except rospy.ServiceException as e:
                rospy.logerr(f"Service call failed: {e}")
        else:
            rospy.loginfo(f'Not ready to call services - Mode {self.last_mode} and Safety {self.last_safety}')


if __name__ == '__main__':
    try:
        monitor = RobotModeMonitor()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass