Merge pull request #2 from deepinbubblegum/humble-develop

Humble develop

moteus_motor/config/params.yaml → moteus_drive/config/params.yaml

@@ -1,4 +1,4 @@
-moteus_motor:
+moteus_drive:
   ros__parameters:
     frame_id: "moteus"
     rezero_on_startup: false

moteus_motor/launch/moteus_node.launch.py → moteus_drive/launch/moteus_node.launch.py

@@ -5,14 +5,14 @@
 def generate_launch_description():
     ld = LaunchDescription()
     config = os.path.join(
-        get_package_share_directory('moteus_motor'),
+        get_package_share_directory('moteus_drive'),
         'config',
         'params.yaml'
     )
 
     node=Node(
-        package = 'moteus_motor',
-        executable = 'moteus_motor',
+        package = 'moteus_drive',
+        executable = 'moteus_drive',
         parameters = [config]
     )
     ld.add_action(node)

moteus_drive/moteus_drive/moteus_control/MoteusDrive.py

@@ -0,0 +1,107 @@
+import asyncio
+import math
+from time import sleep
+import moteus
+from rclpy.node import Node
+
+class MoteusDrive(Node):
+    def __init__(self, IDs):
+        super().__init__('MoteusDriveCycle')
+        self.ids = IDs
+        self.conn = []
+        self.state = "stop"
+        self.terminate = False
+        self.get_logger().info('MoteusDrive: Initialized')
+        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()
+        for idx, device_id in enumerate(self.ids):
+            self.conn.append(moteus.Controller(id = device_id))
+        while True:            
+            if self.state == "stop":
+                self.make_stop = []  
+                for idx, device_id in enumerate(self.ids):
+                    self.make_stop.append(self.conn[idx].make_stop())
+                await transport.cycle(self.make_stop)
+
+            if self.rezero:
+                self.make_rezero = []  
+                for idx, device_id in enumerate(self.ids):
+                    self.make_rezero.append(self.conn[idx].make_rezero())
+                await transport.cycle(self.make_rezero)
+                self.rezero = False
+
+            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"] + self.servo_command[device_id]["velocity"],
+                            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
+
+    def get_feedback(self):
+        return self.raw_feedback
+
+    def run_start(self):
+        asyncio.run(self.run())
+
+    def set_state_start(self):
+        self.state = "start"
+        self.get_logger().info('MoteusDriveState: %s' % self.state)
+
+    def set_state_stop(self):
+        self.state = "stop"
+        self.get_logger().info('MoteusDriveState: %s' % self.state)
+
+    def set_state_rezero(self):
+        self.rezero = True
+        self.get_logger().info('MoteusDriveRezero: %s' % self.rezero)
+
+    def set_state_terminated(self):
+        self.set_state_stop()
+        self.terminate = True
+        self.get_logger().info('MoteusDriveState: %s' % self.state)
+
+    def set_state_brake(self):
+        # self.state = "brake"
+        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_motor/moteus_motor/moteus_node.py → moteus_drive/moteus_drive/moteus_node.py

@@ -1,17 +1,15 @@
 #!/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
-from moteus_motor.moteus_control.MoteusDrive import MoteusDrive
+from moteus_drive.moteus_control.MoteusDrive import MoteusDrive
 import moteus
-from moteus_msgs.msg import MoteusState, MoteusStateStamped
+from moteus_msgs.msg import MoteusState, MoteusStateStamped, MoteusCommandStamped
 
 class MoteusNode(Node):
     def __init__(self):
-        super().__init__('moteus_motor')
+        super().__init__('moteus_drive')
         # declare parameters
         self.declare_param()
 
@@ -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)
-
-        # create timer interval 0.5 second
-        timer_period = 0.01  # seconds
-        self.timer = self.create_timer(timer_period, self.callback_update)
+        self.publisher_ = self.create_publisher(MoteusStateStamped, 'moteus_feedback', 10)
+        self.subscriber_ = self.create_subscription(MoteusCommandStamped, 'moteus_command', self.callback_command, 10)
+
+        # 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):
@@ -76,7 +100,6 @@ def drive_stop(self):
     def drive_terminate(self):
         self.get_logger().info('MoteusNode terminated')
         self.moteusDrive.set_state_terminated()
-        sleep(1)
 
 def main(args=None):
     try:

moteus_motor/package.xml → moteus_drive/package.xml

@@ -1,15 +1,14 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>moteus_motor</name>
+  <name>moteus_drive</name>
   <version>0.0.1</version>
-  <description>moteus_motor</description>
-  <maintainer email="[email protected]">bubble</maintainer>
-  <license>Apache-2.0</license>
+  <description>moteus_drive</description>
+  <maintainer email="[email protected]">chaiwit</maintainer>
+  <license>Apache License 2.0</license>
 
   <depend>rclpy</depend>
 
-
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

moteus_drive/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/moteus_drive
+[install]
+install_scripts=$base/lib/moteus_drive

moteus_motor/setup.py → moteus_drive/setup.py

@@ -2,7 +2,7 @@
 from glob import glob
 from setuptools import setup
 
-package_name = 'moteus_motor'
+package_name = 'moteus_drive'
 
 setup(
     name=package_name,
@@ -21,12 +21,12 @@
     zip_safe=True,
     maintainer='bubble',
     maintainer_email='[email protected]',
-    description='moteus_motor',
-    license='Apache-2.0',
+    description='moteus_drive',
+    license='Apache License 2.0',
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            "moteus_motor = moteus_motor.moteus_node:main"
+            "moteus_drive = moteus_drive.moteus_node:main"
         ],
     },
 )

moteus_msgs/CMakeLists.txt

@@ -11,27 +11,14 @@ find_package(ament_cmake REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 find_package(std_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
-# if(BUILD_TESTING)
-#   find_package(ament_lint_auto REQUIRED)
-#   # the following line skips the linter which checks for copyrights
-#   # comment the line when a copyright and license is added to all source files
-#   set(ament_cmake_copyright_FOUND TRUE)
-#   # the following line skips cpplint (only works in a git repo)
-#   # comment the line when this package is in a git repo and when
-#   # a copyright and license is added to all source files
-#   set(ament_cmake_cpplint_FOUND TRUE)
-#   ament_lint_auto_find_test_dependencies()
-# endif()
-
-# "msg/MoteusCommand.msg"
-# "msg/MoteusCommandStamped.msg"
 set(msg_files
   "msg/MoteusStateStamped.msg"
   "msg/MoteusState.msg"
+  "msg/MoteusCommand.msg"
+  "msg/MoteusCommandStamped.msg"
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}

moteus_msgs/msg/MoteusCommand.msg

@@ -0,0 +1,3 @@
+int32 device_id
+float64 velocity
+float64 maximum_torque

moteus_msgs/msg/MoteusCommandStamped.msg

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