diff --git a/moteus_drive/moteus_drive/moteus_control/MoteusDrive.py b/moteus_drive/moteus_drive/moteus_control/MoteusDrive.py index 514fdeb..89c83a6 100644 --- a/moteus_drive/moteus_drive/moteus_control/MoteusDrive.py +++ b/moteus_drive/moteus_drive/moteus_control/MoteusDrive.py @@ -23,6 +23,7 @@ def __init__(self, Connector, IDs): self.servo_command = None self.feedback_position_device = None self.bus_map = {1: [1], 2: [2]} + self.imu_data = None async def run(self): for idx, device_id in enumerate(self.ids): @@ -32,7 +33,9 @@ async def run(self): if self.Connector == "fdcanusb": transport = moteus.Fdcanusb() elif self.Connector == "pi3hat": - transport = moteus_pi3hat.Pi3HatRouter(servo_bus_map=self.bus_map) + transport = moteus_pi3hat.Pi3HatRouter( + servo_bus_map=self.bus_map + ) for idx, device_id in enumerate(self.ids): self.conn.append(moteus.Controller(id = device_id)) @@ -40,14 +43,20 @@ async def run(self): 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) + self.make_stop.append(self.conn[idx].make_stop(query=True)) + if self.Connector == "pi3hat": + self.set_feedback(await transport.cycle(self.make_stop, request_attitude=True)) + else: + self.set_feedback(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.make_rezero.append(self.conn[idx].make_rezero(query=True)) + if self.Connector == "pi3hat": + self.set_feedback(await transport.cycle(self.make_rezero, request_attitude=True)) + else: + self.set_feedback(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: @@ -61,7 +70,10 @@ async def run(self): query=True ) ) - self.set_feedback(await transport.cycle(self.make_position)) + if self.Connector == "pi3hat": + self.set_feedback(await transport.cycle(self.make_position, request_attitude=True)) + else: + self.set_feedback(await transport.cycle(self.make_position)) await asyncio.sleep(0.01) await asyncio.sleep(0.01) @@ -77,7 +89,10 @@ async def run(self): query=True ) ) - self.set_feedback(await transport.cycle(self.make_brake)) + if self.Connector == "pi3hat": + self.set_feedback(await transport.cycle(self.make_brake, request_attitude=True)) + else: + self.set_feedback(await transport.cycle(self.make_brake)) # self.state = "stop" # self.servo_command = None self.get_logger().info('MoteusDriveState: %s' % self.state) @@ -85,10 +100,27 @@ async def run(self): self.state == "braked" def set_feedback(self, feedback): - self.raw_feedback = feedback + if self.Connector == "pi3hat" and len(feedback) >= 1: + imu_result = [x for x in feedback if x.id == -1 and isinstance(x, moteus_pi3hat.CanAttitudeWrapper)][0] + feedback.pop(-1) + + # att = imu_result.attitude + # print("\n") + # print(f"attitude={att.w:.4f},{att.x:.4f},{att.y:.4f},{att.z:.4f}") + # rate_dps = imu_result.rate_dps + # print(f"rate_dps={rate_dps.x:.3f},{rate_dps.y:.3f},{rate_dps.z:.3f}") + # accel_mps2 = imu_result.accel_mps2 + # print(f"accel_mps2={accel_mps2.x:.3f},{accel_mps2.y:.3f},{accel_mps2.z:.3f}") + # euler_rad = imu_result.euler_rad + # print(f"euler_rad= r={euler_rad.roll:.3f},p={euler_rad.pitch:.3f},y={euler_rad.yaw:.3f}") + + self.raw_feedback = feedback + self.imu_data_feedback = imu_result + else: + self.raw_feedback = feedback def get_feedback(self): - return self.raw_feedback + return self.imu_data_feedback, self.raw_feedback def run_start(self): asyncio.run(self.run()) diff --git a/moteus_drive/moteus_drive/moteus_node.py b/moteus_drive/moteus_drive/moteus_node.py index 8ce553d..f65035b 100644 --- a/moteus_drive/moteus_drive/moteus_node.py +++ b/moteus_drive/moteus_drive/moteus_node.py @@ -1,10 +1,12 @@ #!/usr/bin/python3 import threading +import math import rclpy from rclpy.node import Node from rcl_interfaces.msg import ParameterDescriptor from moteus_drive.moteus_control.MoteusDrive import MoteusDrive import moteus +from sensor_msgs.msg import Imu from moteus_msgs.msg import MoteusState, MoteusStateStamped, MoteusCommandStamped class MoteusNode(Node): @@ -43,7 +45,9 @@ def __init__(self): self.get_logger().info('MoteusNode started') # create publisher for moteus state - self.publisher_ = self.create_publisher(MoteusStateStamped, 'moteus_feedback', 10) + self.publisher_ = self.create_publisher(MoteusStateStamped, 'moteus_feedback', 100) + if self.connector_device == "pi3hat": + self.publisher_imu_ = self.create_publisher(Imu, 'moteus_imu', 100) self.subscriber_ = self.create_subscription(MoteusCommandStamped, 'moteus_command', self.callback_command, 10) # create timer interval @@ -52,7 +56,7 @@ def __init__(self): def declare_param(self): self.declare_parameter("frame_id", "moteus_drive", ParameterDescriptor(description="Frame ID")) - self.declare_parameter("connector_device", "fdcanusb", ParameterDescriptor(description="Conector device")) + self.declare_parameter("connector_device", "pi3hat", ParameterDescriptor(description="Conector device")) self.declare_parameter("rezero_on_startup", False, ParameterDescriptor(description="Rezero on startup")) self.declare_parameter("moteus_ids", [1], ParameterDescriptor(description="Moteus IDs")) @@ -70,29 +74,59 @@ def state_check(self): 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: + imu_feedback, drive_feedback = self.moteusDrive.get_feedback() + if drive_feedback is not None: + stamp = self.get_clock().now().to_msg() moteusStateStamped = MoteusStateStamped() moteusStateStamped.header.frame_id = self.frame_id - moteusStateStamped.header.stamp = self.get_clock().now().to_msg() + moteusStateStamped.header.stamp = stamp for index, device in enumerate(self.devices): moteusStateMsg = MoteusState() moteusStateMsg.device_id = device - moteusStateMsg.mode = feedback[index].values[moteus.Register(device).MODE] - moteusStateMsg.position = feedback[index].values[moteus.Register(device).POSITION] - moteusStateMsg.velocity = feedback[index].values[moteus.Register(device).VELOCITY] - moteusStateMsg.torque = feedback[index].values[moteus.Register(device).TORQUE] - moteusStateMsg.voltage = feedback[index].values[moteus.Register(device).VOLTAGE] - moteusStateMsg.temperature = feedback[index].values[moteus.Register(device).TEMPERATURE] - moteusStateMsg.fault = feedback[index].values[moteus.Register(device).FAULT] + moteusStateMsg.mode = drive_feedback[index].values[moteus.Register(device).MODE] + moteusStateMsg.position = drive_feedback[index].values[moteus.Register(device).POSITION] + moteusStateMsg.velocity = drive_feedback[index].values[moteus.Register(device).VELOCITY] + moteusStateMsg.torque = drive_feedback[index].values[moteus.Register(device).TORQUE] + moteusStateMsg.voltage = drive_feedback[index].values[moteus.Register(device).VOLTAGE] + moteusStateMsg.temperature = drive_feedback[index].values[moteus.Register(device).TEMPERATURE] + moteusStateMsg.fault = drive_feedback[index].values[moteus.Register(device).FAULT] moteusStateStamped.state.append(moteusStateMsg) - self.recv_command[device]["position"] = feedback[index].values[moteus.Register(device).POSITION] + try: + self.recv_command[device]["position"] = drive_feedback[index].values[moteus.Register(device).POSITION] + except: + pass self.publisher_.publish(moteusStateStamped) + if imu_feedback is not None: + imu_data = Imu() + imu_data.header.stamp = stamp + imu_data.header.frame_id = "moteus_imu" + + # Quaternion + att = imu_feedback.attitude + imu_data.orientation.w = att.w + imu_data.orientation.x = att.x + imu_data.orientation.y = att.y + imu_data.orientation.z = att.z + + # Angular velocity + rate_dps = imu_feedback.rate_dps + omega_x = rate_dps.x * math.pi / 180 + omega_y = rate_dps.y * math.pi / 180 + omega_z = rate_dps.z * math.pi / 180 + imu_data.angular_velocity.x = omega_x + imu_data.angular_velocity.y = omega_y + imu_data.angular_velocity.z = omega_z + + # Linear acceleration + accel_mps2 = imu_feedback.accel_mps2 + imu_data.linear_acceleration.x = accel_mps2.x + imu_data.linear_acceleration.y = accel_mps2.y + imu_data.linear_acceleration.z = accel_mps2.z + self.publisher_imu_.publish(imu_data) def drive_rezero(self): if self.rezero_on_startup: