From 81385d0202fa8d48f8919b5793480ee469fb98b3 Mon Sep 17 00:00:00 2001 From: "chaiwit.p" Date: Thu, 15 Sep 2022 20:04:47 +0700 Subject: [PATCH] update --- .../moteus_control/MoteusDrive.py | 54 +++++++++++++++---- moteus_drive/moteus_drive/moteus_node.py | 38 ++++++++++--- moteus_msgs/msg/MoteusCommandStamped.msg | 2 +- 3 files changed, 77 insertions(+), 17 deletions(-) diff --git a/moteus_drive/moteus_drive/moteus_control/MoteusDrive.py b/moteus_drive/moteus_drive/moteus_control/MoteusDrive.py index 15beda6..bc9274a 100644 --- a/moteus_drive/moteus_drive/moteus_control/MoteusDrive.py +++ b/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) \ No newline at end of file + 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" \ No newline at end of file diff --git a/moteus_drive/moteus_drive/moteus_node.py b/moteus_drive/moteus_drive/moteus_node.py index fc5b956..6a87fbb 100644 --- a/moteus_drive/moteus_drive/moteus_node.py +++ b/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): diff --git a/moteus_msgs/msg/MoteusCommandStamped.msg b/moteus_msgs/msg/MoteusCommandStamped.msg index ae35445..eab917d 100644 --- a/moteus_msgs/msg/MoteusCommandStamped.msg +++ b/moteus_msgs/msg/MoteusCommandStamped.msg @@ -1,2 +1,2 @@ std_msgs/Header header -MoteusCommand[] command \ No newline at end of file +MoteusCommand[] commands \ No newline at end of file