update

moteus_drive/moteus_drive/moteus_control/MoteusDrive.py

@@ -1,5 +1,6 @@
 import asyncio
 import math
+from time import sleep
 import moteus
 from rclpy.node import Node
 
@@ -14,6 +15,8 @@ def __init__(self, IDs):
         self.get_logger().info('MoteusDriveState: %s' % self.state)
         self.raw_feedback = None
         self.rezero = False
+        self.servo_command = None
+        self.feedback_position_device = None
 
     async def run(self):
         transport = moteus.Fdcanusb()
@@ -25,7 +28,6 @@ async def run(self):
                 for idx, device_id in enumerate(self.ids):
                     self.make_stop.append(self.conn[idx].make_stop())
                 await transport.cycle(self.make_stop)
-                self.state = "start"
 
             if self.rezero:
                 self.make_rezero = []  
@@ -34,15 +36,40 @@ async def run(self):
                 await transport.cycle(self.make_rezero)
                 self.rezero = False
 
-            while self.terminate is False:
-                if self.state == "start":
-                    self.make_position = []
-                    for idx, device_id in enumerate(self.ids):
-                        self.make_position.append(self.conn[idx].make_position(position=math.nan, velocity=10.0, maximum_torque=1.0, query=True)) 
-                    self.set_feedback(await transport.cycle(self.make_position))
+            while self.terminate is False and self.state == "start" and self.servo_command is not None:
+                self.make_position = []
+                for idx, device_id in enumerate(self.ids):
+                    self.make_position.append(
+                        self.conn[idx].make_position(
+                            position=math.nan, 
+                            velocity=self.servo_command[device_id]["velocity"], 
+                            maximum_torque=self.servo_command[device_id]["maximum_torque"], 
+                            query=True
+                        )
+                    ) 
+                self.set_feedback(await transport.cycle(self.make_position))
                 await asyncio.sleep(0.01)
             await asyncio.sleep(0.01)
 
+            if self.state == "brake" and self.servo_command is not None:
+                self.make_brake = []
+                for idx, device_id in enumerate(self.ids):
+                    self.make_brake.append(
+                        self.conn[idx].make_position(
+                            position = math.nan, 
+                            velocity = 0.0, 
+                            maximum_torque = self.servo_command[device_id]["maximum_torque"],
+                            stop_position = self.servo_command[device_id]["position"] + 20.0,
+                            query=True
+                        )
+                    ) 
+                self.set_feedback(await transport.cycle(self.make_brake))
+                # self.state = "stop"
+                # self.servo_command = None
+                self.get_logger().info('MoteusDriveState: %s' % self.state)
+                await asyncio.sleep(0.01)
+                self.state == "braked"
+
     def set_feedback(self, feedback):
         self.raw_feedback = feedback
 
@@ -52,7 +79,7 @@ def get_feedback(self):
     def run_start(self):
         asyncio.run(self.run())
 
-    def start(self):
+    def set_state_start(self):
         self.state = "start"
         self.get_logger().info('MoteusDriveState: %s' % self.state)
 
@@ -67,4 +94,13 @@ def set_state_rezero(self):
     def set_state_terminated(self):
         self.set_state_stop()
         self.terminate = True
-        self.get_logger().info('MoteusDriveState: %s' % self.state)
+        self.get_logger().info('MoteusDriveState: %s' % self.state)
+
+    def set_state_brake(self):
+        self.state = "stop"
+        self.get_logger().info('MoteusDriveState: %s' % self.state)
+
+    def set_state_command(self, command):
+        self.servo_command = command
+        if self.state != "start":
+            self.state = "start"

moteus_drive/moteus_drive/moteus_node.py

@@ -1,7 +1,5 @@
 #!/usr/bin/python3
-from array import array
 import threading
-from time import sleep
 import rclpy
 from rclpy.node import Node
 from rcl_interfaces.msg import ParameterDescriptor
@@ -20,6 +18,12 @@ def __init__(self):
         self.rezero_on_startup = self.get_parameter("rezero_on_startup").value
         self.devices = self.get_parameter("moteus_ids").value
 
+        # initialize variables and objects
+        self.command = None
+        self.time_command = self.get_clock().now().to_msg()
+        self.time_command_seconds = self.get_clock().now().to_msg()
+        self.recv_command = {}
+
         # initialize moteus drive cycle
         self.moteusDrive = MoteusDrive(self.devices)
 
@@ -35,18 +39,37 @@ def __init__(self):
         self.get_logger().info('MoteusNode started')
 
         # create publisher for moteus state
-        self.publisher_ = self.create_publisher(MoteusStateStamped, 'MoteusFeedback', 10)
+        self.publisher_ = self.create_publisher(MoteusStateStamped, 'moteus_feedback', 10)
+        self.subscriber_ = self.create_subscription(MoteusCommandStamped, 'moteus_command', self.callback_command, 10)
 
-        # create timer interval 0.5 second
-        timer_period = 0.01  # seconds
-        self.timer = self.create_timer(timer_period, self.callback_update)
+        # create timer interval
+        self.timer_state = self.create_timer(0.1, self.state_check)
+        self.timer_update = self.create_timer(0.01, self.interval_update)
 
     def declare_param(self):
         self.declare_parameter("frame_id", "moteus_drive", ParameterDescriptor(description="Frame ID"))
         self.declare_parameter("rezero_on_startup", False, ParameterDescriptor(description="Rezero on startup"))
         self.declare_parameter("moteus_ids",[1], ParameterDescriptor(description="Moteus IDs"))
 
-    def callback_update(self):
+    def callback_command(self, msg):
+        self.time_command = msg.header.stamp
+        self.commands = msg.commands
+        self.recv_command = {}
+        for idx, command in enumerate(self.commands):
+            self.recv_command[command.device_id] = {"velocity": command.velocity, "maximum_torque": command.maximum_torque, "position": None}
+        # send commands
+        self.moteusDrive.set_state_command(self.recv_command)
+
+    def state_check(self):
+        now_seconds = self.get_clock().now().to_msg()
+        now_seconds = float(str(now_seconds.sec) + '.' + str(now_seconds.nanosec))
+        self.time_command_seconds = float(str(self.time_command.sec) + '.' + str(self.time_command.nanosec))
+        timeout = now_seconds - self.time_command_seconds
+        print(timeout)
+        if timeout >= 1: #timeout 1 second emergency stop
+            self.moteusDrive.set_state_brake()
+
+    def interval_update(self):
         feedback = self.moteusDrive.get_feedback()
         if feedback is not None:
             moteusStateStamped = MoteusStateStamped()
@@ -63,6 +86,7 @@ def callback_update(self):
                 moteusStateMsg.temperature = feedback[index].values[moteus.Register(device).TEMPERATURE]
                 moteusStateMsg.fault = feedback[index].values[moteus.Register(device).FAULT]
                 moteusStateStamped.state.append(moteusStateMsg)
+                self.recv_command[device]["position"] = feedback[index].values[moteus.Register(device).POSITION]
             self.publisher_.publish(moteusStateStamped)
 
     def drive_rezero(self):

moteus_msgs/msg/MoteusCommandStamped.msg

@@ -1,2 +1,2 @@
 std_msgs/Header header
-MoteusCommand[] command
+MoteusCommand[] commands