unity2moveit2/scripts/configure-ros-connection.ps1

# Configure ROS2 Connection Settings for Unity Project
# 为Unity项目配置ROS2连接设置

param(
    [string]$ProjectPath = "D:\ros2\DockerRos2Arm-niryo_arm\Unity_MoveIt2\NewUnityProject\ROS2_Niryo_Control",
    [string]$UnityPath = "C:\Program Files\Unity\Hub\Editor\2021.3.33f1c1\Editor\Unity.exe",
    [string]$ROSIPAddress = "localhost",
    [int]$ROSPort = 10000,
    [switch]$OpenProject,
    [switch]$CreateTestScene
)

Write-Host "=== Configuring ROS2 Connection Settings ===" -ForegroundColor Green

# Check if Unity project exists
if (-not (Test-Path $ProjectPath)) {
    Write-Host "Error: Unity project not found at $ProjectPath" -ForegroundColor Red
    exit 1
}

# Create ROS Configuration Manager script
$rosConfigScript = @"
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;

namespace ROS2NiryoControl
{
    [System.Serializable]
    public class ROSConnectionSettings
    {
        [Header("Connection Settings")]
        public string rosIPAddress = "localhost";
        public int rosPort = 10000;
        
        [Header("Unity Settings")]
        public string unityIPAddress = "0.0.0.0";
        public int unityPort = 5005;
        
        [Header("Timeout Settings")]
        public float connectionTimeout = 10f;
        public int maxRetries = 3;
        public float retryDelay = 2f;
    }

    public class ROSConnectionManager : MonoBehaviour
    {
        [Header("ROS Connection Configuration")]
        public ROSConnectionSettings connectionSettings = new ROSConnectionSettings();
        
        [Header("Status")]
        public bool isConnected = false;
        public string connectionStatus = "Disconnected";
        
        private ROSConnection rosConnection;
        private float lastConnectionCheck = 0f;
        private int connectionAttempts = 0;
        
        void Start()
        {
            InitializeROSConnection();
        }
        
        void InitializeROSConnection()
        {
            // Get or create ROS connection instance
            rosConnection = ROSConnection.GetOrCreateInstance();
            
            // Configure connection settings
            rosConnection.RosIPAddress = connectionSettings.rosIPAddress;
            rosConnection.RosPort = connectionSettings.rosPort;
            rosConnection.ConnectOnStart = true;
            
            // Start connection monitoring
            InvokeRepeating(nameof(CheckConnectionStatus), 1f, 2f);
            
            Debug.Log($"ROS Connection initialized - Target: {connectionSettings.rosIPAddress}:{connectionSettings.rosPort}");
        }
        
        void CheckConnectionStatus()
        {
            bool wasConnected = isConnected;
            isConnected = rosConnection != null && rosConnection.HasConnectionThread && rosConnection.IsConnected.Value;
            
            if (isConnected && !wasConnected)
            {
                connectionStatus = "Connected";
                connectionAttempts = 0;
                Debug.Log("✓ ROS2 Connection established successfully!");
                OnConnectionEstablished();
            }
            else if (!isConnected && wasConnected)
            {
                connectionStatus = "Connection Lost";
                Debug.LogWarning("⚠ ROS2 Connection lost!");
                OnConnectionLost();
            }
            else if (!isConnected)
            {
                connectionAttempts++;
                connectionStatus = $"Connecting... (Attempt {connectionAttempts})";
                
                if (connectionAttempts >= connectionSettings.maxRetries)
                {
                    connectionStatus = "Connection Failed";
                    Debug.LogError("✗ Failed to establish ROS2 connection after multiple attempts");
                }
            }
        }
        
        void OnConnectionEstablished()
        {
            // Override this method in derived classes for custom behavior
            Debug.Log("ROS2 connection established - ready for robot control");
        }
        
        void OnConnectionLost()
        {
            // Override this method in derived classes for custom behavior
            Debug.LogWarning("ROS2 connection lost - robot control disabled");
        }
        
        public void ReconnectROS()
        {
            connectionAttempts = 0;
            connectionStatus = "Reconnecting...";
            
            if (rosConnection != null)
            {
                // Force reconnection
                rosConnection.RosIPAddress = connectionSettings.rosIPAddress;
                rosConnection.RosPort = connectionSettings.rosPort;
            }
            
            Debug.Log("Attempting to reconnect to ROS2...");
        }
        
        void OnGUI()
        {
            // Simple status display
            GUILayout.BeginArea(new Rect(10, 10, 400, 120));
            GUILayout.BeginVertical("box");
            
            GUILayout.Label("ROS2 Connection Status", EditorStyles.boldLabel);
            GUILayout.Label($"Status: {connectionStatus}");
            GUILayout.Label($"Target: {connectionSettings.rosIPAddress}:{connectionSettings.rosPort}");
            GUILayout.Label($"Connected: {(isConnected ? "Yes" : "No")}");
            
            if (!isConnected && GUILayout.Button("Reconnect"))
            {
                ReconnectROS();
            }
            
            GUILayout.EndVertical();
            GUILayout.EndArea();
        }
        
        void OnApplicationQuit()
        {
            if (rosConnection != null)
            {
                Debug.Log("Closing ROS2 connection...");
            }
        }
    }
}
"@

$rosConfigPath = Join-Path $ProjectPath "Assets\Scripts\ROS\ROSConnectionManager.cs"
Write-Output $rosConfigScript | Out-File -FilePath $rosConfigPath -Encoding UTF8
Write-Host "✓ ROS Connection Manager script created" -ForegroundColor Green

# Create Robot Controller script
$robotControllerScript = @"
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.UrdfImporter.Control;

namespace ROS2NiryoControl.Robot
{
    public class NiryoRobotController : MonoBehaviour
    {
        [Header("Robot Configuration")]
        public GameObject niryoRobot;
        public ArticulationBody[] joints;
        
        [Header("Control Settings")]
        public float jointSpeed = 30f;
        public float jointAcceleration = 10f;
        
        [Header("ROS Topics")]
        public string jointStatesTopic = "/joint_states";
        public string jointCommandTopic = "/joint_commands";
        
        private ROSConnectionManager rosConnectionManager;
        private bool isInitialized = false;
        
        void Start()
        {
            InitializeRobotController();
        }
        
        void InitializeRobotController()
        {
            // Find ROS connection manager
            rosConnectionManager = FindObjectOfType<ROSConnectionManager>();
            if (rosConnectionManager == null)
            {
                Debug.LogError("ROSConnectionManager not found! Please add it to the scene.");
                return;
            }
            
            // Initialize robot joints if not set
            if (joints == null || joints.Length == 0)
            {
                if (niryoRobot != null)
                {
                    joints = niryoRobot.GetComponentsInChildren<ArticulationBody>();
                    Debug.Log($"Found {joints.Length} articulation joints in robot");
                }
            }
            
            // Configure joint properties
            ConfigureJoints();
            
            isInitialized = true;
            Debug.Log("Niryo Robot Controller initialized successfully");
        }
        
        void ConfigureJoints()
        {
            foreach (var joint in joints)
            {
                if (joint.jointType != ArticulationJointType.FixedJoint)
                {
                    // Configure joint drive
                    var drive = joint.xDrive;
                    drive.stiffness = 10000f;
                    drive.damping = 100f;
                    drive.forceLimit = 1000f;
                    joint.xDrive = drive;
                    
                    // Set joint friction
                    joint.jointFriction = 0.1f;
                    joint.angularDamping = 0.1f;
                }
            }
        }
        
        public void MoveJointToPosition(int jointIndex, float targetPosition)
        {
            if (!isInitialized || jointIndex >= joints.Length) return;
            
            var joint = joints[jointIndex];
            if (joint.jointType != ArticulationJointType.FixedJoint)
            {
                var drive = joint.xDrive;
                drive.target = targetPosition * Mathf.Rad2Deg; // Convert to degrees
                joint.xDrive = drive;
            }
        }
        
        public void MoveAllJoints(float[] targetPositions)
        {
            if (!isInitialized || targetPositions.Length != joints.Length) return;
            
            for (int i = 0; i < targetPositions.Length; i++)
            {
                MoveJointToPosition(i, targetPositions[i]);
            }
        }
        
        public float[] GetCurrentJointPositions()
        {
            if (!isInitialized) return new float[0];
            
            float[] positions = new float[joints.Length];
            for (int i = 0; i < joints.Length; i++)
            {
                if (joints[i].jointType != ArticulationJointType.FixedJoint)
                {
                    positions[i] = joints[i].jointPosition[0] * Mathf.Deg2Rad; // Convert to radians
                }
            }
            return positions;
        }
        
        void OnGUI()
        {
            if (!isInitialized) return;
            
            GUILayout.BeginArea(new Rect(10, 140, 400, 200));
            GUILayout.BeginVertical("box");
            
            GUILayout.Label("Robot Controller", EditorStyles.boldLabel);
            GUILayout.Label($"Joints: {joints.Length}");
            GUILayout.Label($"Robot: {(niryoRobot != null ? niryoRobot.name : "Not Set")}");
            
            if (rosConnectionManager != null && rosConnectionManager.isConnected)
            {
                GUILayout.Label("Status: Ready for Control", EditorStyles.miniLabel);
            }
            else
            {
                GUILayout.Label("Status: Waiting for ROS Connection", EditorStyles.miniLabel);
            }
            
            GUILayout.EndVertical();
            GUILayout.EndArea();
        }
    }
}
"@

$robotControllerPath = Join-Path $ProjectPath "Assets\Scripts\Robot\NiryoRobotController.cs"
Write-Output $robotControllerScript | Out-File -FilePath $robotControllerPath -Encoding UTF8
Write-Host "✓ Niryo Robot Controller script created" -ForegroundColor Green

# Create test scene if requested
if ($CreateTestScene) {
    Write-Host "Creating test scene configuration..." -ForegroundColor Yellow
    
    $testSceneScript = @"
using UnityEngine;
using UnityEngine.SceneManagement;

namespace ROS2NiryoControl
{
    public class TestSceneSetup : MonoBehaviour
    {
        [Header("Scene Objects")]
        public Camera mainCamera;
        public Light mainLight;
        public GameObject ground;
        
        void Start()
        {
            SetupTestScene();
        }
        
        void SetupTestScene()
        {
            // Configure camera
            if (mainCamera == null)
            {
                mainCamera = Camera.main;
            }
            
            if (mainCamera != null)
            {
                mainCamera.transform.position = new Vector3(0, 1.5f, -2f);
                mainCamera.transform.rotation = Quaternion.Euler(15f, 0f, 0f);
                mainCamera.backgroundColor = Color.gray;
            }
            
            // Configure lighting
            if (mainLight == null)
            {
                mainLight = FindObjectOfType<Light>();
            }
            
            if (mainLight != null)
            {
                mainLight.type = LightType.Directional;
                mainLight.intensity = 1.2f;
                mainLight.transform.rotation = Quaternion.Euler(45f, 45f, 0f);
            }
            
            Debug.Log("Test scene setup completed");
        }
    }
}
"@

    $testScenePath = Join-Path $ProjectPath "Assets\Scripts\UI\TestSceneSetup.cs"
    Write-Output $testSceneScript | Out-File -FilePath $testScenePath -Encoding UTF8
    Write-Host "✓ Test Scene Setup script created" -ForegroundColor Green
}

# Update ROS connection settings in existing scripts
Write-Host "Updating connection settings..." -ForegroundColor Yellow

# Create configuration file
$configContent = @"
# ROS2 Connection Configuration
# ROS2连接配置

# Docker Container Settings
ROS_IP_ADDRESS=localhost
ROS_PORT=10000

# Unity Settings  
UNITY_IP_ADDRESS=0.0.0.0
UNITY_PORT=5005

# Connection Settings
CONNECTION_TIMEOUT=10
MAX_RETRIES=3
RETRY_DELAY=2

# Robot Settings
ROBOT_NAME=niryo_one
JOINT_COUNT=6
CONTROL_FREQUENCY=50

# Topics
JOINT_STATES_TOPIC=/joint_states
JOINT_COMMANDS_TOPIC=/joint_commands
TRAJECTORY_TOPIC=/follow_joint_trajectory
"@

$configPath = Join-Path $ProjectPath "Assets\Resources\ros_config.txt"
Write-Output $configContent | Out-File -FilePath $configPath -Encoding UTF8
Write-Host "✓ ROS configuration file created" -ForegroundColor Green

# Open Unity project if requested
if ($OpenProject) {
    Write-Host "Opening Unity project..." -ForegroundColor Yellow
    Start-Process -FilePath $UnityPath -ArgumentList "-projectPath", $ProjectPath
    Write-Host "✓ Unity project opened" -ForegroundColor Green
}

Write-Host "`n=== ROS2 Connection Configuration Completed ===" -ForegroundColor Green
Write-Host "Project Path: $ProjectPath" -ForegroundColor White
Write-Host ""
Write-Host "Created Components:" -ForegroundColor Yellow
Write-Host "✓ ROSConnectionManager - Main connection handler" -ForegroundColor White
Write-Host "✓ NiryoRobotController - Robot control interface" -ForegroundColor White
Write-Host "✓ TestSceneSetup - Scene configuration helper" -ForegroundColor White
Write-Host "✓ ROS Configuration file - Connection settings" -ForegroundColor White
Write-Host ""
Write-Host "Connection Settings:" -ForegroundColor Yellow
Write-Host "• ROS IP: $ROSIPAddress" -ForegroundColor White
Write-Host "• ROS Port: $ROSPort" -ForegroundColor White
Write-Host "• Unity Port: 5005" -ForegroundColor White
Write-Host ""
Write-Host "Next Steps:" -ForegroundColor Yellow
Write-Host "1. Open Unity project and wait for compilation" -ForegroundColor White
Write-Host "2. Add ROSConnectionManager to your scene" -ForegroundColor White
Write-Host "3. Import Niryo robot URDF" -ForegroundColor White
Write-Host "4. Add NiryoRobotController to robot GameObject" -ForegroundColor White
Write-Host "5. Test connection with Docker ROS2 container" -ForegroundColor White

Write-Host "`nConfiguration completed successfully!" -ForegroundColor Green