private void ListenForData() { try { socketConnection = new TcpClient(remoteIP, 13579); if (socketConnection.Connected) { Debug.Log("TCP Server connected."); } while (true) { using (NetworkStream stream = socketConnection.GetStream()) { Byte[] bytes = new Byte[1024]; // Read incomming stream into byte arrary. while (true) { try { TargetJSON t = new TargetJSON(); t.status = status; t.x = target_position.x; t.y = target_position.y; t.z = -target_position.z; // t.yaw = target_orientation.y / 180f * Mathf.PI; t.yaw = target_orientation.y; Byte[] send_msg = Encoding.UTF8.GetBytes(JsonUtility.ToJson(t)); // Byte[] send_msg = Encoding.UTF8.GetBytes("RECV"); stream.Write(send_msg, 0, send_msg.Length); Array.Clear(bytes, 0, bytes.Length); stream.Read(bytes, 0, bytes.Length); string json_str = Encoding.UTF8.GetString(bytes); // print(json_str); PoseJSON p = JsonUtility.FromJson <PoseJSON>(json_str); current_position.x = p.x; current_position.y = p.y; current_position.z = -p.z; current_orientation.x = -p.pitch * 180f / Mathf.PI; current_orientation.y = p.yaw * 180f / Mathf.PI; current_orientation.z = p.roll * 180f / Mathf.PI; } catch (Exception e) { stream.Flush(); // Debug.Log(e); } } } } } catch (SocketException socketException) { Debug.Log("Socket exception: " + socketException); } }
private void ListenForData() { try { socketConnection = new TcpClient(remoteIP, 13500); if (socketConnection.Connected) { Debug.Log("TCP Server connected."); } while (true) { // Get a stream object for reading using (NetworkStream stream = socketConnection.GetStream()) { Byte[] length_bytes = new Byte[16]; // Read incomming stream into byte arrary. while (true) { try { stream.Read(length_bytes, 0, length_bytes.Length); int length = Convert.ToInt32(Encoding.ASCII.GetString(length_bytes)); var bytes = new byte[length]; stream.Read(bytes, 0, bytes.Length); Byte[] send_msg = Encoding.UTF8.GetBytes("RECV"); stream.Write(send_msg, 0, send_msg.Length); string json_str = Encoding.UTF8.GetString(bytes); // Debug.Log(json_str); PoseJSON p = JsonUtility.FromJson <PoseJSON>(json_str); // Debug.Log(p.position.x); // string[] pose_str = serverMessage.Split(','); float x = p.position.x; float y = p.position.y; float z = p.position.z; Vector3 ros_current_position = new Vector3(x, y, z); // Coord conversion from ROS to Unity should have been done here. if (isSimulation) { // Gazebo current_position.x = ros_current_position.x; current_position.y = ros_current_position.z; current_position.z = ros_current_position.y; } else { // [TEST OK] 3DR current_position.x = -ros_current_position.y; current_position.y = ros_current_position.z; current_position.z = ros_current_position.x; // [TEST OK] RealSense current_position.x = ros_current_position.x; current_position.y = ros_current_position.y; current_position.z = -ros_current_position.z; } Vector3 ros_current_euler_orientation = (new Quaternion(p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w)).eulerAngles; // Vector3 ros_current_euler_orientation = new Vector3(); // ros_current_euler_orientation.x = p.position.x; // ros_current_euler_orientation.y = p.position.x); // ros_current_euler_orientation.z = p.position.x); // From ROS_PoseStamped.cs euler.y, -euler.z , -euler.x // x = y, y = -z, z = -x [TEST NOT OK] // MY TRANSFORMATION x=-x, y=-z, z=-y (Right-handed to Left-handed) if (isSimulation) { // [TEST OK] on Simulation current_orientation.x = -ros_current_euler_orientation.x; current_orientation.y = -ros_current_euler_orientation.z; current_orientation.z = -ros_current_euler_orientation.y; } else { // [TEST OK] on 3DR current_orientation.x = ros_current_euler_orientation.y; current_orientation.y = -ros_current_euler_orientation.z; current_orientation.z = -ros_current_euler_orientation.x; // [TEST OK] on RealSense current_orientation.x = -ros_current_euler_orientation.x; current_orientation.y = -ros_current_euler_orientation.y; current_orientation.z = -(-ros_current_euler_orientation.z); } // Debug.Log("current_position " + current_position); } catch (Exception e) { Debug.Log(e); } } } } } catch (SocketException socketException) { Debug.Log("Socket exception: " + socketException); } }
private void ListenForData() { try { socketConnection = new TcpClient(remoteIP, 13579); if (socketConnection.Connected) { Debug.Log("TCP Server connected."); } while (true) { using (NetworkStream stream = socketConnection.GetStream()) { Byte[] bytes = new Byte[1024]; // Read incomming stream into byte arrary. while (true) { try { TargetJSON t = new TargetJSON(); t.status = status; t.x = target_position.x; t.y = target_position.y; t.z = -target_position.z; // t.yaw = target_orientation.y / 180f * Mathf.PI; t.yaw = target_orientation.y; Byte[] send_msg = Encoding.UTF8.GetBytes(JsonUtility.ToJson(t)); // Byte[] send_msg = Encoding.UTF8.GetBytes("RECV"); stream.Write(send_msg, 0, send_msg.Length); Array.Clear(bytes, 0, bytes.Length); stream.Read(bytes, 0, bytes.Length); string json_str = Encoding.UTF8.GetString(bytes); print(json_str); PoseJSON p = JsonUtility.FromJson <PoseJSON>(json_str); byte[] msg = Encoding.UTF8.GetBytes("NEXTFRAME"); stream.Write(msg, 0, msg.Length); Byte[] length_bytes; length_bytes = new Byte[16]; stream.Read(length_bytes, 0, 16); int length = Convert.ToInt32(Encoding.ASCII.GetString(length_bytes)); Debug.Log("Receiving data of length = " + length.ToString()); Byte[] full_buff = new Byte[length]; int current_length = 0; int chunk_length; Byte[] buff = new Byte[buff_size]; while (stream.CanRead && current_length < length) { while (stream.DataAvailable && (chunk_length = stream.Read(buff, 0, buff_size)) != 0) { for (int i = 0; i < chunk_length; i++) { full_buff[i + current_length] = buff[i]; } current_length += chunk_length; } } // JPEG Format Check if (full_buff[0] == 255 && full_buff[1] == 216) { // image_bytes = full_buff; Debug.Log(frame_queue.Count); frame_queue.Enqueue(full_buff); } current_position.x = p.x; current_position.y = p.y; current_position.z = -p.z; current_orientation.x = -p.pitch * 180f / Mathf.PI; current_orientation.y = p.yaw * 180f / Mathf.PI; current_orientation.z = p.roll * 180f / Mathf.PI; } catch (Exception e) { stream.Flush(); // Debug.Log(e); } } } } } catch (SocketException socketException) { Debug.Log("Socket exception: " + socketException); } }