kerrigan_json.py

Go to the documentation of this file.

#!/usr/bin/env python
 
# Author: Yu Zhou
# Date: Feb 22, 2019
 
import sys
import rospy
#import cv2
from nav_msgs.msg import Odometry
import nav_msgs
import std_msgs.msg
import geometry_msgs.msg
from rospy_message_converter import json_message_converter
import json
import socket
import math
import nus_msgs
import std_srvs.srv
import time
from nus_msgs.msg import StateWithCovarianceStamped
from common_msgs.msg import target
from sensor_msgs.msg import NavSatFix
# from std_msgs import Bool
from tf.transformations import quaternion_from_euler, euler_from_quaternion
from geometry_msgs.msg import Quaternion, Point, PoseStamped, PointStamped, Vector3Stamped, Polygon
from std_srvs.srv import Trigger, TriggerRequest
 
 
 
class SwarmSocket:
    def readFile(self, filename):
        file = open(filename, 'r')
        fileText = file.read()
        file.close()
        return fileText
 
 
    def writeFile(self, filename, text):
        file = open(filename, 'w')
        file.write(text)
        return
 
    def UDPServerSetup(self, ip_addr = "127.0.0.1", port = 1234):
        # Set up a UDP server
        self.UDPSock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        listen_addr = (ip_addr, port)
        self.UDPSock.bind(listen_addr)
        return
 
    def sendToRemote(self,text):
        UDP_IP = "127.0.0.1"               # remote ip
        UDP_PORT = 4866
        #print "UDP target IP:", UDP_IP
        #print "UDP target port:", UDP_PORT
        sock = socket.socket(socket.AF_INET,  # Internet
                             socket.SOCK_DGRAM)  # UDP
        #print(text)
        sock.sendto(text, (UDP_IP, UDP_PORT))
 
    def __init__(self):
        self.preSep = 0
        self.curSeq = 0
        self.localIPAddress = "127.0.0.1"   # own ip address
        self.localPort = 9028                  
        self.vCmdPub = rospy.Publisher('/velTgtFromSwarm', Point, queue_size=10)
        self.pCmdPub = rospy.Publisher('/posTgtFromSwarm', Point, queue_size=10)
        self.approachPub = rospy.Publisher('/swarm/approach', std_msgs.msg.Bool, queue_size=1)
        self.onlinePub = rospy.Publisher('/swarm/online', std_msgs.msg.Bool, queue_size=1)
        # for simulation
        # rospy.Subscriber("/hil/state/ground_truth", StateWithCovarianceStamped, self.uav_state_CB)
        # for flight testing
        rospy.Subscriber("/mavros/position/local_state", StateWithCovarianceStamped, self.uav_state_CB)
        rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.gpsPosCb)
        rospy.Subscriber("/task_manager/swarm", std_msgs.msg.Bool, self.swarmCb)
        rospy.Subscriber("/car_tracking", std_msgs.msg.Float32MultiArray, self.carTrackingCb)
        # rospy.Subscriber("/engage_cmd",Bool, self.typeTrigger)
        self.UDPServerSetup(self.localIPAddress, self.localPort)
        self.takeoff_srv = rospy.Service('/takeoffdone', Trigger, self.trigger_response)
        self.response = std_srvs.srv.TriggerResponse()
        self.response.success = False
        # self.takeoff = False
        self.jsonDic = {}
        self.jsonSwarmDic = {}
  
 
    def __enter__(self):
        return self
 
    def __exit__(self, type, value, traceback):
        self.UDPSock.close()
        return self
 
    # def typeTrigger(self, data):
    #     self.takeoff = data.data
    #     return
 
    def swarmCb(self, data):
        data = json_message_converter.convert_ros_message_to_json(data)
        swarmDic = json.loads(data.strip())
        print("Send swarm to CA: ", swarmDic["data"])
        self.jsonSwarmDic['type'] = "swarm"
        self.jsonSwarmDic['flag'] = swarmDic["data"]
        # self.jsonSwarmDic['time'] = int(round(time.time() * 1000))
        jsonSendStr = json.dumps(self.jsonSwarmDic)
        self.sendToRemote(jsonSendStr)
        return
 
    def carTrackingCb(self, data):
        index = 0
        target_num = len(data.data)/4
        print("Car Number: ", target_num)
        data = json_message_converter.convert_ros_message_to_json(data)
        print(data)
        jsonCarTrackDic = {}
        carTrackDic = json.loads(data.strip())
        jsonCarTrackDic['type'] = "targettrack"
        jsonCarTrackDic['result']=[]
        for index in range(target_num):
            jsonCarTrackDic['result'].append({"class_id": int(carTrackDic["data"][index]), "confidence":carTrackDic["data"][index+1], "u": carTrackDic["data"][index+2], "v":carTrackDic["data"][index+3]})
        
        jsonSendStr = json.dumps(jsonCarTrackDic)
        self.sendToRemote(jsonSendStr)
        return
 
 
    def uav_state_CB(self, data):
        data = json_message_converter.convert_ros_message_to_json(data)
        uavStateDic = json.loads(data.strip())
        q = [uavStateDic["pose"]["pose"]["orientation"]["x"], 
        uavStateDic["pose"]["pose"]["orientation"]["y"],
        uavStateDic["pose"]["pose"]["orientation"]["z"],
        uavStateDic["pose"]["pose"]["orientation"]["w"]]
        euler = euler_from_quaternion(q)
        self.jsonDic['roll'] = euler[0]*57.3  
        self.jsonDic['pitch'] = euler[1]*57.3  
        self.jsonDic['yaw'] = euler[2]*57.3  
        # jsonDic['x'] = uavStateDic["pose"]["pose"]["position"]["x"]
        # jsonDic['y'] = uavStateDic["pose"]["pose"]["position"]["y"]
        # self.jsonDic['altitude'] = uavStateDic["pose"]["pose"]["position"]["z"]
        self.jsonDic['u'] = uavStateDic["velocity"]["twist"]["linear"]["x"]
        self.jsonDic['v'] = uavStateDic["velocity"]["twist"]["linear"]["y"]
        self.jsonDic['w'] = uavStateDic["velocity"]["twist"]["linear"]["z"]
        self.jsonDic['time'] = int(round(time.time() * 1000)) #uavStateDic["header"]["seq"]
        # jsonSendStr = json.dumps(self.jsonDic)
        # self.sendToRemote(jsonSendStr)
        #print(jsonSendStr)
        return
 
 
    def gpsPosCb(self, data):
        data = json_message_converter.convert_ros_message_to_json(data)
        uavStateDic = json.loads(data.strip())
        
        self.jsonDic['latitude'] = uavStateDic["latitude"]
        self.jsonDic['longitude'] = uavStateDic["longitude"]
        self.jsonDic['altitude'] = uavStateDic["altitude"]
        self.jsonDic['time'] = int(round(time.time() * 1000))
        self.jsonDic['type'] = "UAVPosVel"
 
        jsonSendStr = json.dumps(self.jsonDic)
        self.sendToRemote(jsonSendStr)
        #print(jsonSendStr)
        return
 
    def publishNavMessage(self, jsonMsg):
        if 'time' in jsonMsg:
            if self.preSep == 0:
                self.preSep = int(jsonMsg["time"]) 
            self.curSeq = int(jsonMsg["time"])
            if self.curSeq < self.preSep:
                return
            self.preSep = self.curSeq
        
        cmdMsg = geometry_msgs.msg.Point()
        if jsonMsg['type'] == 'velocity':  
            cmdMsg.x = float(jsonMsg["u"]) 
            cmdMsg.y = float(jsonMsg["v"])
            cmdMsg.z = float(jsonMsg["w"])
            self.vCmdPub.publish(cmdMsg)
            # print("pub traj once!")
        if jsonMsg['type'] == 'position':
            cmdMsg.x = float(jsonMsg["latitude"]) 
            cmdMsg.y = float(jsonMsg["longitude"])
            cmdMsg.z = float(jsonMsg["altitude"])
            self.pCmdPub.publish(cmdMsg)
 
        if jsonMsg['type'] == 'approach':
            approachMsg = std_msgs.msg.Bool()
            approachMsg.data = jsonMsg["flag"]
            print("approach flag:", approachMsg.data)
            if approachMsg.data:
                self.approachPub.publish(approachMsg)
 
        if jsonMsg['type'] == 'online':
            onlineMsg = std_msgs.msg.Bool()
            onlineMsg.data = jsonMsg["flag"]
            #print("online flag:", onlineMsg.data)
            if not onlineMsg.data:
                self.onlinePub.publish(onlineMsg)
 
        else:
            return
 
    def trigger_response(self, request):
        self.response.success = True
        self.response.message = "take off done"
        return self.response
 
 
    def remoteInterfaceModule(self):
        rospy.init_node('SwarmSocket', log_level= rospy.DEBUG)
        while not rospy.is_shutdown():
            data, addr = self.UDPSock.recvfrom(self.localPort)
            receivedJSONMessage = json.loads(data.strip()) # convert json string to a dictionary
            # print(receivedJSONMessage)
            self.publishNavMessage(receivedJSONMessage)
            #unityROSMessage = json_message_converter.convert_json_to_ros_message("nav_msgs/Odometry", receivedJSONMessage)               
            #rospy.sleep(5.)
        return
    
def main(args):
    swarm_socket = SwarmSocket()
    swarm_socket.remoteInterfaceModule()
    #print('Destroying all the windows...')
    #cv2.destroyAllWindows()
 
if __name__ == '__main__':
    main(sys.argv)