Python-BDI-Experiments/ros_bridge_client.py at master · ronsm/Python-BDI-Experiments · 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
import colorama
from colorama import Fore, Style
import threading
import time
import requests

class ros_bridge_client(object):
    def __init__(self, belief_manager):
        self.agent_id = 'ros_bridge_client'
        self.log('init')

        self.belief_manager = belief_manager

        self.ros_server_url = 'http://192.168.175.141:8888/'

    def send(self, agent, service, args, result_recipient):
        url = self.ros_server_url + 'service_request'

        agent = str(agent)
        service = str(service)
        args = str(args)

        form = {
            'agent' : agent,
            'service' : service,
            'args' : args
        }
        r = requests.post(url, data=form)
        response = r.text
        self.log(response)

        t = threading.Thread(target =  self.spawn_watchdog, args = (service, response, result_recipient))
        t.start()

    def spawn_watchdog(self, service, response, result_recipient):
        url = self.ros_server_url + 'service_request_status'
        form = {
            'client_id' : response
        }

        status = -1
        while(status == -1):
            log_msg = 'waiting on a response to service request: ' + response + ' for agent: ' + result_recipient
            self.log(log_msg)

            r = requests.post(url, data=form)
            r = r.text
            status = int(r)

            time.sleep(2)

        log_msg = 'response received for service request: ' + response + ' for agent: ' + result_recipient + ', response = ' + str(status)
        self.log(log_msg)

        self.belief_manager.send_to_agent(result_recipient, service, status, self.agent_id)

    def log(self, msg):
        tag = '[' + self.agent_id + ']'
        print(Fore.CYAN + tag, Fore.RESET + msg)