Skip to content

Commit

Permalink
Merge pull request #6 from deepinbubblegum/humble-develop
Browse files Browse the repository at this point in the history 
Humble develop
  • Loading branch information
@deepinbubblegum
deepinbubblegum authored Sep 18, 2022
2 parents 927a202 + 0822348 commit 334629b
Show file tree
Hide file tree
Showing 2 changed files with 89 additions and 23 deletions.
50 changes: 41 additions & 9 deletions moteus_drive/moteus_drive/moteus_control/MoteusDrive.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand All @@ -32,22 +33,30 @@ 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))
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)
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:
Expand All @@ -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)

Expand All @@ -77,18 +89,38 @@ 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)
await asyncio.sleep(0.01)
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())
Expand Down
62 changes: 48 additions & 14 deletions moteus_drive/moteus_drive/moteus_node.py
View file
Original file line number Diff line number Diff line change
@@ -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):
Expand Down Expand Up @@ -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
Expand All @@ -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"))

Expand All @@ -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:
Expand Down

0 comments on commit 334629b

Please sign in to comment.