diff --git a/moteus_motor/config/params.yaml b/moteus_drive/config/params.yaml similarity index 88% rename from moteus_motor/config/params.yaml rename to moteus_drive/config/params.yaml index f94dcb4..d6dc437 100644 --- a/moteus_motor/config/params.yaml +++ b/moteus_drive/config/params.yaml @@ -1,4 +1,4 @@ -moteus_motor: +moteus_drive: ros__parameters: frame_id: "moteus" rezero_on_startup: false diff --git a/moteus_motor/launch/moteus_node.launch.py b/moteus_drive/launch/moteus_node.launch.py similarity index 75% rename from moteus_motor/launch/moteus_node.launch.py rename to moteus_drive/launch/moteus_node.launch.py index 284835a..47077da 100644 --- a/moteus_motor/launch/moteus_node.launch.py +++ b/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) diff --git a/moteus_motor/moteus_motor/__init__.py b/moteus_drive/moteus_drive/__init__.py similarity index 100% rename from moteus_motor/moteus_motor/__init__.py rename to moteus_drive/moteus_drive/__init__.py diff --git a/moteus_drive/moteus_drive/moteus_control/MoteusDrive.py b/moteus_drive/moteus_drive/moteus_control/MoteusDrive.py new file mode 100644 index 0000000..c4064c6 --- /dev/null +++ b/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" \ No newline at end of file diff --git a/moteus_motor/moteus_motor/moteus_control/__init__.py b/moteus_drive/moteus_drive/moteus_control/__init__.py similarity index 100% rename from moteus_motor/moteus_motor/moteus_control/__init__.py rename to moteus_drive/moteus_drive/moteus_control/__init__.py diff --git a/moteus_motor/moteus_motor/moteus_node.py b/moteus_drive/moteus_drive/moteus_node.py similarity index 65% rename from moteus_motor/moteus_motor/moteus_node.py rename to moteus_drive/moteus_drive/moteus_node.py index 1d70861..6a87fbb 100644 --- a/moteus_motor/moteus_motor/moteus_node.py +++ b/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: diff --git a/moteus_motor/package.xml b/moteus_drive/package.xml similarity index 72% rename from moteus_motor/package.xml rename to moteus_drive/package.xml index 44b9052..42919ca 100644 --- a/moteus_motor/package.xml +++ b/moteus_drive/package.xml @@ -1,15 +1,14 @@ - moteus_motor + moteus_drive 0.0.1 - moteus_motor - bubble - Apache-2.0 + moteus_drive + chaiwit + Apache License 2.0 rclpy - ament_copyright ament_flake8 ament_pep257 diff --git a/moteus_motor/resource/moteus_motor b/moteus_drive/resource/moteus_drive similarity index 100% rename from moteus_motor/resource/moteus_motor rename to moteus_drive/resource/moteus_drive diff --git a/moteus_drive/setup.cfg b/moteus_drive/setup.cfg new file mode 100644 index 0000000..4edaf62 --- /dev/null +++ b/moteus_drive/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/moteus_drive +[install] +install_scripts=$base/lib/moteus_drive diff --git a/moteus_motor/setup.py b/moteus_drive/setup.py similarity index 82% rename from moteus_motor/setup.py rename to moteus_drive/setup.py index b630259..93c8142 100644 --- a/moteus_motor/setup.py +++ b/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='chaiwit.p@groupmaker.co.th', - 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" ], }, ) diff --git a/moteus_motor/test/test_copyright.py b/moteus_drive/test/test_copyright.py similarity index 100% rename from moteus_motor/test/test_copyright.py rename to moteus_drive/test/test_copyright.py diff --git a/moteus_motor/test/test_flake8.py b/moteus_drive/test/test_flake8.py similarity index 100% rename from moteus_motor/test/test_flake8.py rename to moteus_drive/test/test_flake8.py diff --git a/moteus_motor/test/test_pep257.py b/moteus_drive/test/test_pep257.py similarity index 100% rename from moteus_motor/test/test_pep257.py rename to moteus_drive/test/test_pep257.py diff --git a/moteus_motor/moteus_motor/moteus_control/MoteusDrive.py b/moteus_motor/moteus_motor/moteus_control/MoteusDrive.py deleted file mode 100644 index 607ccba..0000000 --- a/moteus_motor/moteus_motor/moteus_control/MoteusDrive.py +++ /dev/null @@ -1,70 +0,0 @@ -import asyncio -import math -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 - - def set_feedback(self, feedback): - self.raw_feedback = feedback - - def get_feedback(self): - return self.raw_feedback - - async def run(self): - transport = moteus.Fdcanusb() - for id in self.ids: - self.conn.append(moteus.Controller(id = id)) - while True: - if self.state == "stop": - self.make_stop = [] - for id in range(len(self.ids)): - self.make_stop.append(self.conn[id].make_stop()) - await transport.cycle(self.make_stop) - self.state = "start" - - if self.rezero: - self.make_rezero = [] - for id in range(len(self.ids)): - self.make_rezero.append(self.conn[id].make_rezero()) - await transport.cycle(self.make_rezero) - self.rezero = False - - while self.terminate is False: - if self.state == "start": - self.make_position = [] - for id in range(len(self.ids)): - self.make_position.append(self.conn[id].make_position(position=math.nan, velocity=10.0, maximum_torque=1.0, query=True)) - self.set_feedback(await transport.cycle(self.make_position)) - await asyncio.sleep(0.01) - await asyncio.sleep(0.01) - - def run_start(self): - asyncio.run(self.run()) - - def 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) \ No newline at end of file diff --git a/moteus_motor/setup.cfg b/moteus_motor/setup.cfg deleted file mode 100644 index 43c2479..0000000 --- a/moteus_motor/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/moteus_motor -[install] -install_scripts=$base/lib/moteus_motor diff --git a/moteus_msgs/CMakeLists.txt b/moteus_msgs/CMakeLists.txt index 542ecda..d2e0872 100644 --- a/moteus_msgs/CMakeLists.txt +++ b/moteus_msgs/CMakeLists.txt @@ -11,27 +11,14 @@ find_package(ament_cmake REQUIRED) # further dependencies manually. # find_package( 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} diff --git a/moteus_msgs/msg/MoteusCommand.msg b/moteus_msgs/msg/MoteusCommand.msg index e69de29..126f053 100644 --- a/moteus_msgs/msg/MoteusCommand.msg +++ b/moteus_msgs/msg/MoteusCommand.msg @@ -0,0 +1,3 @@ +int32 device_id +float64 velocity +float64 maximum_torque \ No newline at end of file diff --git a/moteus_msgs/msg/MoteusCommandStamped.msg b/moteus_msgs/msg/MoteusCommandStamped.msg index 18f51f8..eab917d 100644 --- a/moteus_msgs/msg/MoteusCommandStamped.msg +++ b/moteus_msgs/msg/MoteusCommandStamped.msg @@ -1 +1,2 @@ -Header Header \ No newline at end of file +std_msgs/Header header +MoteusCommand[] commands \ No newline at end of file diff --git a/moteus_msgs/package.xml b/moteus_msgs/package.xml index 5a62689..b225d15 100644 --- a/moteus_msgs/package.xml +++ b/moteus_msgs/package.xml @@ -4,8 +4,8 @@ moteus_msgs 0.0.1 Moteus message - bubble - Apache-2.0 + chaiwit + Apache License 2.0 ament_cmake