Set up a subscriber in Winch.cs for ROS2 control auto-winch function

Runtime/Scripts/ROS/Subscribers/Winch_Sub.cs

@@ -0,0 +1,89 @@
+using UnityEngine;
+using Unity.Robotics.ROSTCPConnector;
+using RosMessageTypes.Std;
+using VehicleComponents.ROS.Core; 
+
+using Rope;
+
+namespace VehicleComponents.ROS.Subscribers
+{
+    [RequireComponent(typeof(Winch))]
+    public class Winch_Sub : ROSBehaviour
+    {
+        Winch winch;
+
+        protected override void StartROS()
+        {
+            Debug.Log("[ROS] Winch_Sub StartROS called");
+            winch = GetComponent<Winch>();
+            rosCon.Subscribe<Float32MultiArrayMsg>("/winch_control_test", HandleTestControl);
+        }
+
+
+        void HandleTestControl(Float32MultiArrayMsg msg)
+        {
+            if (msg.data.Length < 2) return;
+
+            float target = msg.data[0];
+            float speed = msg.data[1];
+            winch.TargetLength = Mathf.Clamp(target, winch.MinLength, winch.RopeLength);
+            winch.WinchSpeed = speed;
+        }
+    }
+}
+
+// using UnityEngine;
+// using RosMessageTypes.Std;
+// using VehicleComponents.ROS.Core;
+// using Rope;
+
+// namespace VehicleComponents.ROS.Subscribers
+// {
+//     [RequireComponent(typeof(Winch))]
+//     public class Winch_Sub : Actuator_Sub<Float32MultiArrayMsg>
+//     {
+//         private Winch winch;
+
+//         protected override void StartROS()
+//         {
+//             base.StartROS();  // Ensure base subscription logic runs
+
+//             winch = GetComponent<Winch>();
+//             if (winch == null)
+//             {
+//                 Debug.LogError("[ROS] Winch component not found on GameObject.");
+//                 enabled = false;
+//                 return;
+//             }
+
+//             Debug.Log("[ROS] Winch_Sub StartROS completed");
+//         }
+
+//         protected override void UpdateVehicle(bool reset)
+//         {
+//             if (reset)
+//             {
+//                 Debug.LogWarning("[ROS] Winch_Sub reset triggered due to low message frequency.");
+//                 return; // Optional: implement a reset behavior here
+//             }
+
+//             var data = ROSMsg.data;
+//             if (data == null || data.Length < 2)
+//             {
+//                 Debug.LogWarning("[ROS] Invalid Float32MultiArray received in Winch_Sub.");
+//                 return;
+//             }
+
+//             float target = data[0];
+//             float speed = data[1];
+
+//             winch.TargetLength = Mathf.Clamp(target, winch.MinLength, winch.RopeLength);
+//             winch.WinchSpeed = speed;
+
+//             // Optional debug:
+//             Debug.Log($"[ROS] Winch command received: target={target}, speed={speed}");
+//         }
+//     }
+// }
+
+

Runtime/Scripts/Rope/RopeHook.cs

@@ -1,5 +1,6 @@
 using DefaultNamespace;
 using UnityEngine;
+//using VehicleComponents.ROS.Publishers; // Needed for TF publishing
 
 
 namespace Rope
@@ -15,13 +16,25 @@ public class RopeHook : MonoBehaviour
         public GameObject PulleyGO;
 
 
+        [Header("TF Settings")]
+        [Tooltip("TF frame name for the hook.")]
+        public string HookTFFrame = "Hook";
+
         [Header("Debug")]
         public bool DrawForces = false;
 
         [Tooltip("If true, the hook will attach to the base_link of the given buoy on start.")]
         public bool AttachToRopeLinkAfterStart = false;
         public RopeLinkBuoy RopeLinkBuoy;
 
+        // void Start()
+        // {
+        //     // Add TF publisher for the hook
+        //     var tfPub = gameObject.AddComponent<ROSTransformTreePublisher>();
+        //     tfPub.BaseLinkName = HookTFFrame; // "Hook"
+        //     Debug.Log($"Hook TF frame set: {HookTFFrame}");
+        // }
+
         bool connectedToBuoy = false;
 
         void FixedUpdate()

Runtime/Scripts/Rope/Winch.cs

@@ -1,7 +1,8 @@
 using UnityEngine;
 
 using Force;
-
+using Unity.Robotics.ROSTCPConnector;
+using StdMessages = RosMessageTypes.Std;
 namespace Rope
 {
     public class Winch : RopeSystemBase
@@ -33,6 +34,29 @@ public class Winch : RopeSystemBase
         [Header("Debug")]
         public float ActualDistance;
 
+        ROSConnection ros;
+
+        string winchFeedbackTopic = "/winch_control_unity";
+        float CurrentRopeSpeedFeedback;
+        float CurrentLengthFeedback;
+        float TargetLengthFeedback;
+
+        void WinchControlTestCallback(StdMessages.Float32MultiArrayMsg msg)
+        {
+            if (msg.data.Length >= 2)
+            {
+                float testTargetLength = msg.data[0];
+                float testWinchSpeed = msg.data[1];
+                Debug.Log($"Received test Float32MultiArray: target_length={testTargetLength}, winch_speed={testWinchSpeed}");
+
+                TargetLength = Mathf.Clamp(testTargetLength, MinLength, RopeLength);
+                WinchSpeed = testWinchSpeed;
+            }
+            else
+            {
+                Debug.LogWarning("Received Float32MultiArray with insufficient data.");
+            }
+        }
 
 
 
@@ -69,18 +93,42 @@ void OnValidate()
 
         void Awake()
         {
-            if (loadBody == null) loadBody = new MixedBody(LoadAB, LoadRB);
+            ros = ROSConnection.GetOrCreateInstance();
+            Debug.Log("Subscribing to /winch_control_test");
+            ros.Subscribe<StdMessages.Float32MultiArrayMsg>("/winch_control_test", WinchControlTestCallback);
+
+            // Register publisher
+            ros.RegisterPublisher<StdMessages.Float32MultiArrayMsg>(winchFeedbackTopic);
+
+            //if (loadBody == null) loadBody = new MixedBody(LoadAB, LoadRB);
         }
 
         void Update()
         {
-            if(!setup) return;
+            if (!setup) return;
             ActualDistance = Vector3.Distance(loadBody.position, transform.position);
             bool ropeSlack = ActualDistance < CurrentLength;
             lineRenderer.SetPosition(0, transform.position);
             lineRenderer.SetPosition(1, loadBody.position);
             lineRenderer.startColor = ropeSlack ? Color.green : Color.red;
             lineRenderer.endColor = lineRenderer.startColor;
+
+
+            // Update feedback values
+            CurrentRopeSpeedFeedback = CurrentRopeSpeed;
+            TargetLengthFeedback = TargetLength;
+            CurrentLengthFeedback = CurrentLength;
+
+            PublishWinchFeedback();
+
+        }
+
+
+        void PublishWinchFeedback()
+        {
+            StdMessages.Float32MultiArrayMsg feedbackMsg = new StdMessages.Float32MultiArrayMsg();
+            feedbackMsg.data = new float[] { CurrentRopeSpeedFeedback, TargetLengthFeedback, CurrentLengthFeedback };
+            ros.Publish(winchFeedbackTopic, feedbackMsg);
         }
 
         void FixedUpdate()
@@ -95,9 +143,9 @@ void FixedUpdate()
             ropeCollider.transform.rotation = Quaternion.LookRotation(toLoad.normalized, transform.forward);
             ropeCollider.height = toLoad.magnitude;
 
-            // simple speed control
+            // simple speed control   
             var lenDiff = TargetLength - CurrentLength;
-            if (Mathf.Abs(lenDiff) > 0.025)
+            if(Mathf.Abs(lenDiff) > 0.015)   // > 0.025  too small will cause the winch control jitering
             {
                 CurrentRopeSpeed = lenDiff > 0 ? WinchSpeed : -WinchSpeed;
             }