merged master

NaviGator/gnc/navigator_path_planner/nodes/path_planner.py

@@ -1,7 +1,5 @@
 #!/usr/bin/env python3
 
-import time
-import threading
 from types import ModuleType
 from typing import List, Optional
 
@@ -152,21 +150,8 @@ def __init__(
         # Timers
         # rospy.Timer(rospy.Duration(self.revisit_period), self.publish_ref)
         rospy.Timer(rospy.Duration(self.revisit_period), self.action_check)
-        self.custom_task = threading.Thread(target=self.simple_timer)
-        self.custom_task.start()
-
-    def simple_timer(self):
-        while not rospy.is_shutdown():
-            start = time.perf_counter()
-            # print("start! ", start)
-            self.publish_ref()
-            end = time.perf_counter()
-            # print(f"taken {end - start}s")
-            time_to_sleep = max(0.03 - (end - start), 0)
-            target = time.perf_counter() + time_to_sleep
-            time.sleep(0.03)
-            # while time.perf_counter() < target:
-            #     pass
+        # self.custom_task = threading.Thread(target=self.simple_timer)
+        # self.custom_task.start()
 
     def reset(self) -> None:
         """
@@ -1398,8 +1383,8 @@ def publish_ref(self, *args) -> None:
         publisher. Also publishes the reference effort as a WrenchStamped
         message to the effort publisher.
         """
-        import datetime
-        start = time.time()
+
+        # start = time.time()
         # Make sure a plan exists
         last_update_time = self.last_update_time
         if self.get_ref is None or last_update_time is None:

NaviGator/scripts/thruster_spin.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+
+import argparse
+import threading
+
+import rospy
+from roboteq_msgs.msg import Command
+
+
+def publish_thruster_command(thruster, rate):
+    topic = f"/{thruster}_motor/cmd"
+    command = Command(mode=0, setpoint=float(rate))
+    pub = rospy.Publisher(topic, Command, queue_size=10)
+    rate_obj = rospy.Rate(rate)
+    rospy.loginfo(f"Starting to publish to {topic} with setpoint: {rate}")
+    while not rospy.is_shutdown():
+        pub.publish(command)
+        rate_obj.sleep()
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description="Spin thrusters for the autonomous boat",
+    )
+
+    parser.add_argument("--all", action="store_true", help="Spin all thrusters")
+    parser.add_argument("--rate", type=float, default=0, help="Set thruster spin rate")
+    parser.add_argument("--slow", action="store_true", help="Set slow rate (50)")
+    parser.add_argument("--medium", action="store_true", help="Set medium rate (100)")
+    parser.add_argument("--fast", action="store_true", help="Set fast rate (200)")
+    parser.add_argument(
+        "thrusters",
+        nargs="*",
+        help="List of thruster names to spin (FR, FL, BR, BL)",
+    )
+
+    args = parser.parse_args()
+
+    # Define default rates based on arguments
+    if args.slow:
+        rate = 50
+    elif args.medium:
+        rate = 100
+    elif args.fast:
+        rate = 200
+    elif args.rate > 0:
+        rate = args.rate
+    else:
+        rospy.logerr("No rate specified! Use --rate, --slow, --medium, or --fast.")
+        return
+
+    # Initialize ROS node
+    rospy.init_node("thruster_spin_control", anonymous=True)
+
+    # Determine thrusters to spin
+    thrusters = []
+    if args.all:
+        thrusters = ["FR", "FL", "BR", "BL"]
+    elif args.thrusters:
+        thrusters = args.thrusters
+    else:
+        rospy.logerr(
+            "No thrusters specified! Use --all or list specific thrusters (FR, FL, BR, BL).",
+        )
+        return
+
+    # Start spinning each thruster in a separate thread
+    threads = []
+    for thruster in thrusters:
+        rospy.loginfo(f"Starting {thruster} at rate {rate}")
+        thread = threading.Thread(
+            target=publish_thruster_command,
+            args=(thruster, rate),
+        )
+        thread.start()
+        threads.append(thread)
+
+    # Wait for all threads to complete
+    for thread in threads:
+        thread.join()
+
+
+if __name__ == "__main__":
+    main()