kuri_mbzirc_challenge_2/smach_actionlib_server_template.py at master · kucars/kuri_mbzirc_challenge_2 · 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
#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
Created on 10/04/2016
@author: Abdullah Abduldayem (template)
"""

import rospy
import actionlib
import math
import sys
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import Point
from sensor_msgs.msg import Image
from kuri_mbzirc_challenge_2_msgs.msg import WrenchDetectionAction

node_name = "wrench_detection_server"
topic_name = "wrench_detection"

class WrenchDetectionServer:

    def __init__(self, is_bypass):
        ##########
        ## Initialize state machine interface
        ##########
        self.is_bypass = is_bypass

        self.is_node_enabled = False
        if (self.is_bypass):
            self.is_node_enabled = True

        # Start actionlib server
        self.server = actionlib.SimpleActionServer(topic_name, WrenchDetectionAction, self.execute, False)
        self.server.start()

        rospy.loginfo("Started " + node_name + " node. Currently on standby")

        ##########
        ## Initialize node
        ##########
        # Variables
        self.variable = 0

        # Set up callbacks
        self.camera_sub = rospy.Subscriber('/camera', Image, self.camera_callback, queue_size=20)


    def execute(self, goal_msg):
        # This node was called, perform any necessary changes here
        self.is_node_enabled = True
        rospy.loginfo(node_name + " node enabled")


    def camera_callback(self, data):
        # Return if node is not enabled
        if (not self.is_node_enabled):
            return


        # Node is enabled, process the camera data

        # ...


        # If the wrenches are detected, return the region of interest
        p1 = Point(0 ,0 ,0)
        p2 = Point(10,0 ,0)
        p3 = Point(10,10,0)
        p4 = Point(0 ,10,0)

        rospy.loginfo("Found wrench")
        result = WrenchDetectionResult()

        result.ROI = [p1, p2, p3, p4]
        self.server.set_succeeded(result)

        # Disable the node since it found its target
        if (not self.is_bypass):
            self.is_node_enabled = False


if __name__ == '__main__':
      rospy.init_node(node_name)

      is_bypass = False
      if len(sys.argv) >= 2:
          is_bypass = True

      server = WrenchDetectionServer(is_bypass)
      rospy.spin()