senseAndAvoid_json.py

Go to the documentation of this file.

#!/usr/bin/env python
 
# Author: Yu Zhou
# Date: Feb 22, 2019
"""@package 
communication middleware between ros and non-ros modules
 
Designed for kerrigan object originally
"""
 
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
import common_msgs.msg 
from sensor_msgs.msg import NavSatFix
import tf.transformations
# from std_msgs import Bool
from tf.transformations import quaternion_from_euler, euler_from_quaternion
from geometry_msgs.msg import Quaternion, Point, TwistStamped, PoseStamped, PointStamped, Vector3Stamped, Polygon
from std_srvs.srv import Trigger, TriggerRequest
import threading
import numpy as np
 
 
class SwarmSocket:
    def readFile(self, filename):
        """read from file"""
        file = open(filename, 'r')
        fileText = file.read()
        file.close()
        return fileText
 
 
    def writeFile(self, filename, text):
        """write to file"""
 
        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):
        """send text to remote machine """
        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', common_msgs.msg.target, queue_size=1)
        self.pCmdPub = rospy.Publisher('/posTgtFromSwarm', common_msgs.msg.target, queue_size=1)
        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/local_position/velocity_local", TwistStamped, self.VelCb)
        rospy.Subscriber("/task_manager/position_nwu", PoseStamped, self.PoseCb)
        rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.gpsPosCb)
        rospy.Subscriber("/task_manager/swarm", std_msgs.msg.Bool, self.swarmCb)
        rospy.Subscriber("/detector/result", std_msgs.msg.Float32MultiArray, self.senseCb)
        # 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.jsonSwarmDic = {}
        self.file = open("/media/nvidia/SD/catkin_ws/data/detection_result", "w")
        self.file.write('x_body, y_body, z_body, x_global, y_global, z_global, roll, pitch, yaw\n')
        self.jsonDic = {"sense":False, "type": "SenseAvoid" }
        self.q = [0,0,0,1]
        self.sendOtherUAVInfo()
 
        
  
    def sendOtherUAVInfo(self):
        """ send other uav's info with detection result """
        # print "got other uav info"
        sending_thread = threading.Timer(0.05, self.sendOtherUAVInfo)
        jsonSendStr = json.dumps(self.jsonDic)
        # print(jsonSendStr)
        # self.file.write(jsonSendStr)
        self.sendToRemote(jsonSendStr)
        sending_thread.start()
 
    def __enter__(self):
        return self
 
    def __exit__(self, type, value, traceback):
        self.UDPSock.close()
        return self
 
    def swarmCb(self, data):
        """get swarm control info from /task_manager/swarm """
 
        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 senseCb(self, data):
        """get detection result from /detector/result"""
 
        index = 0
        target_num = len(data.data)/4
        data = json_message_converter.convert_ros_message_to_json(data)
        
        droneDic = json.loads(data.strip())
        
        for index in range(target_num):
            # self.jsonDic['other_x'] = droneDic["data"][index+1]
            # self.jsonDic['other_y'] = droneDic["data"][index+2]
            # self.jsonDic['other_z'] = droneDic["data"][index+3]
            self.jsonDic['sense'] = bool(droneDic["data"][index])
            # print('flag: ', self.jsonDic['sense'])
            if np.isinf(droneDic["data"][index+1]) or np.isinf(droneDic["data"][index+2]) or np.isinf(droneDic["data"][index+3]):
                return
            other_body = [droneDic["data"][index+3], -droneDic["data"][index+1], -droneDic["data"][index+2],0]
            # print('other_body: ', other_body)
            # print('q: ', self.q)
            other_global = self.qv_mult(self.q, other_body)
            # print('other_global: ', other_global)
 
            self.jsonDic['other_body_x'] = other_body[0]
            self.jsonDic['other_body_y'] = other_body[1]
            self.jsonDic['other_body_z'] = other_body[2]
 
            self.jsonDic['other_x'] = other_global[0]
            self.jsonDic['other_y'] = other_global[1]
            self.jsonDic['other_z'] = other_global[2]
            self.jsonDic['h_pixel_dist'] = droneDic["data"][index+4]
            # print(self.jsonDic)
            # json.dumps(self.jsonDic, self.file)
            self.file.write('%s, %s, %s, %s, %s, %s, %s, %s, %s\n' %(other_body[0], other_body[1], other_body[2],other_global[0], other_global[1], other_global[2],self.jsonDic['roll'], self.jsonDic['pitch'] ,self.jsonDic['yaw'] ))
        
        return
 
    def PoseCb(self, data):
        """ get position from /task_manager/position_nwu"""
        data = json_message_converter.convert_ros_message_to_json(data)
        uavStateDic = json.loads(data.strip())
        self.q = [uavStateDic["pose"]["orientation"]["x"], 
        uavStateDic["pose"]["orientation"]["y"],
        uavStateDic["pose"]["orientation"]["z"],
        uavStateDic["pose"]["orientation"]["w"]]
        euler = euler_from_quaternion(self.q)
        self.jsonDic['roll'] = euler[0] 
        self.jsonDic['pitch'] = euler[1] 
        self.jsonDic['yaw'] = euler[2]
        # print(self.jsonDic)
        return
 
    def VelCb(self, data):
        """get velocity from /mavros/local_position/velocity_local"""
        data = json_message_converter.convert_ros_message_to_json(data)
        uavStateDic = json.loads(data.strip())
 
        self.jsonDic['u'] = uavStateDic["twist"]["linear"]["y"]
        self.jsonDic['v'] = -uavStateDic["twist"]["linear"]["x"]
        self.jsonDic['w'] = uavStateDic["twist"]["linear"]["z"]
        self.jsonDic['time'] = int(round(time.time() * 1000)) #uavStateDic["header"]["seq"]
        # print(len(self.jsonDic))
        return
 
    def qv_mult(self, q1, v1):
        """transform vector v1 through q1"""
        # v1 = tf.transformations.unit_vector(v1)
        # q2 = list(v1)
        # q2.append(0.0)
        return tf.transformations.quaternion_multiply(
            tf.transformations.quaternion_multiply(q1, v1), 
            tf.transformations.quaternion_conjugate(q1)
        )[:3]
 
    def gpsPosCb(self, data):
        """get global pose from /mavros/global_position/global"""
        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))
        return
 
    def publishNavMessage(self, jsonMsg):
        """sent all the reqired info to non-ros CA module"""
        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()
        cmdMsg = common_msgs.msg.target()
 
        if jsonMsg['type'] == 'velocity': 
            cmdMsg.controller = True 
            cmdMsg.tgt.x = float(jsonMsg["u"]) 
            cmdMsg.tgt.y = float(jsonMsg["v"])
            cmdMsg.tgt.z = float(jsonMsg["w"])
            cmdMsg.yaw_tgt = float(jsonMsg["yaw"])
            self.vCmdPub.publish(cmdMsg)
            # print("pub traj once!")
        if jsonMsg['type'] == 'position':
            cmdMsg.controller = False
            cmdMsg.tgt.x = float(jsonMsg["latitude"]) 
            cmdMsg.tgt.y = float(jsonMsg["longitude"])
            cmdMsg.tgt.z = float(jsonMsg["altitude"])
            cmdMsg.yaw_tgt = float(jsonMsg["yaw"])
            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):
        """get notified of taking off status through /takeoffdone"""
        self.response.success = True
        self.response.message = "take off done"
        return self.response
 
 
    def remoteInterfaceModule(self):
        """Info receving and sending"""
        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()
 
if __name__ == '__main__':
    main(sys.argv)