Art + Robotics Research LAB Forum - Recent Posts https://robartx.tech/community/ Art + Robotics Research LAB Discussion Board en-US Sat, 13 Jun 2026 06:16:40 +0000 wpForo 60 RE: code https://robartx.tech/community/robocall/code/#post-36 Fri, 27 Feb 2026 19:30:57 +0000
roboclaw_node.py
 
#!/usr/bin/env python
import diagnostic_updater
import rclpy
from geometry_msgs.msg import Twist
from rclpy.node import Node

from tcr_roboclaw import Roboclaw
from .electrical_wrapper import ElectricalWrapper
from .encoder_wrapper import EncoderWrapper
from . import utils as u

__author__ = "bwbazemore@uga.edu (Brad Bazemore)"

import serial # raw serial helper (used only in apply_voltage_limits)

# -----------------------------
# Raw RoboClaw packet helpers
# -----------------------------
def _crc16_ccitt(packet: bytes) -> int:
"""RoboClaw CRC16-CCITT (poly 0x1021), init 0."""
crc = 0
for b in packet:
crc ^= (b << 8)
for _ in range(8):
if crc & 0x8000:
crc = ((crc << 1) ^ 0x1021) & 0xFFFF
else:
crc = (crc << 1) & 0xFFFF
return crc


def _send_packet_expect_ff(
ser: serial.Serial, payload_wo_crc: bytes, timeout_s: float = 0.2
) -> bool:
"""Send payload (addr+cmd+data), append CRC16. Expect 0xFF ack."""
crc = _crc16_ccitt(payload_wo_crc)
pkt = payload_wo_crc + crc.to_bytes(2, "big")
ser.reset_input_buffer()
ser.write(pkt)
ser.flush()
ser.timeout = timeout_s
ack = ser.read(1)
return ack == b"\xFF"


def _read_exact(ser: serial.Serial, n: int, timeout_s: float = 0.3) -> bytes:
ser.timeout = timeout_s
data = ser.read(n)
if len(data) != n:
raise RuntimeError(
f"Timeout: expected {n} bytes, got {len(data)} bytes: {data!r}"
)
return data


def _set_main_voltages_raw(
ser: serial.Serial, addr: int, vmin_tenths: int, vmax_tenths: int, offset_tenths: int
) -> bool:
"""
57 - Set Main Battery Voltages
Send:
Receive:
"""
payload = bytearray()
payload += int(vmin_tenths).to_bytes(2, "big")
payload += int(vmax_tenths).to_bytes(2, "big")
payload += bytes()
return _send_packet_expect_ff(ser, bytes(payload))


def _set_logic_voltages_raw(
ser: serial.Serial, addr: int, vmin_tenths: int, vmax_tenths: int
) -> bool:
"""
58 - Set Logic Battery Voltages
Send:
Receive:
"""
payload = bytearray()
payload += int(vmin_tenths).to_bytes(2, "big")
payload += int(vmax_tenths).to_bytes(2, "big")
return _send_packet_expect_ff(ser, bytes(payload))


def _read_main_voltage_settings_raw(ser: serial.Serial, addr: int):
"""
59 - Read Main Battery Voltage Settings
Receive:
CRC computed over
"""
cmd = bytes()
ser.reset_input_buffer()
ser.write(cmd + _crc16_ccitt(cmd).to_bytes(2, "big"))
ser.flush()

resp = _read_exact(ser, 2 + 2 + 1 + 2)
data = resp
recv_crc = int.from_bytes(resp, "big")
check = _crc16_ccitt(bytes() + data)
if recv_crc != check:
raise RuntimeError(
f"Bad CRC on ReadMainVoltageSettings: recv {recv_crc:04X} != calc {check:04X}"
)

minv = int.from_bytes(data, "big")
maxv = int.from_bytes(data, "big")
off = data
return (minv, maxv, off)


def _read_logic_voltage_settings_raw(ser: serial.Serial, addr: int):
"""
60 - Read Logic Battery Voltage Settings
Receive:
CRC over
"""
cmd = bytes()
ser.reset_input_buffer()
ser.write(cmd + _crc16_ccitt(cmd).to_bytes(2, "big"))
ser.flush()

resp = _read_exact(ser, 2 + 2 + 2)
data = resp
recv_crc = int.from_bytes(resp, "big")
check = _crc16_ccitt(bytes() + data)
if recv_crc != check:
raise RuntimeError(
f"Bad CRC on ReadLogicVoltageSettings: recv {recv_crc:04X} != calc {check:04X}"
)

minv = int.from_bytes(data, "big")
maxv = int.from_bytes(data, "big")
return (minv, maxv)


class RoboclawNode(Node):
"""RoboClaw Node"""

def apply_voltage_limits(self):
min_main_t = int(round(float(self.min_main_voltage) * 10))
max_main_t = int(round(float(self.max_main_voltage) * 10))
min_logic_t = int(round(float(self.min_logic_voltage) * 10))
max_logic_t = int(round(float(self.max_logic_voltage) * 10))
offset_t = int(round(float(self.automax_main_offset_voltage) * 10))
offset_t = max(0, min(255, offset_t))

baud = int(self.BAUD)

targets =
if self.MIRROR_SECOND:
targets.append((str(self.DEV2), int(self.ADDRESS2)))

for port, addr in targets:
self.get_logger().info(
f"Applying RoboClaw voltage limits on {port} @ {baud}, addr={addr}: "
f"MAIN {min_main_t/10:.1f}-{max_main_t/10:.1f}V (offset {offset_t/10:.1f}V), "
f"LOGIC {min_logic_t/10:.1f}-{max_logic_t/10:.1f}V"
)

try:
with serial.Serial(port, baud, timeout=0.3) as ser:
ok_main = _set_main_voltages_raw(ser, addr, min_main_t, max_main_t, offset_t)
ok_logic = _set_logic_voltages_raw(ser, addr, min_logic_t, max_logic_t)

if not ok_main:
self.get_logger().warn(f" Set Main Voltage (57) did not ACK (0xFF).")
if not ok_logic:
self.get_logger().warn(f" Set Logic Voltage (58) did not ACK (0xFF).")

try:
main_min, main_max, main_off = _read_main_voltage_settings_raw(ser, addr)
logic_min, logic_max = _read_logic_voltage_settings_raw(ser, addr)
self.get_logger().info(
f" Voltage settings now: "
f"MAIN {main_min/10:.1f}-{main_max/10:.1f}V (offset {main_off/10:.1f}V), "
f"LOGIC {logic_min/10:.1f}-{logic_max/10:.1f}V"
)
except Exception as e:
self.get_logger().warn(f" Could not read back voltage settings (59/60): {e}")

except Exception as e:
self.get_logger().warn(f" Failed to apply voltage limits: {e}")

def __init__(self):
super().__init__("roboclaw_node")

self.read_parameters()

# Board 1
self.driver = self.init_device(self.DEV, self.ADDRESS, self.BAUD)

# Board 2 (optional mirror)
self.driver2 = None
if self.MIRROR_SECOND:
self.driver2 = self.init_device(self.DEV2, self.ADDRESS2, self.BAUD)

# Apply voltage limits (one-time at startup)
self.apply_voltage_limits()

# Reset + configure both boards
self.reset_device()
self.configure_device()

self.create_subscribers()
self.create_wrappers()
self.create_timers()

self.updater = diagnostic_updater.Updater(self)
self.updater.setHardwareID("RoboClaw")
self.updater.add(diagnostic_updater.FunctionDiagnosticTask("Vitals", self.diagnostics))

self.last_cmd_timestamp = self.get_clock().now().nanoseconds

# NEW: for encoder debug logging throttling
self._last_enc_log_ns = 0

def read_parameters(self):
"""Read parameters from the parameter server"""
self.get_logger().info("Reading parameters...")

# Device params
self.DEV = self.init_parameter("dev", "/dev/ttyACM0")
self.BAUD = self.init_parameter("baud", 38400)
self.ADDRESS = self.init_parameter("address", 128)
self.serial_timeout = self.init_parameter("serial_timeout", 0.0)
if self.ADDRESS > 0x87 or self.ADDRESS < 0x80:
self.get_logger().fatal("Address out of range")
self.shutdown("Address out of range")

# Odometry params
self.TICKS_PER_METER = self.init_parameter("ticks_per_meter", 4342.2)
self.TICKS_PER_ROTATION = self.init_parameter("ticks_per_rotation", 2780)
self.BASE_WIDTH = self.init_parameter("base_width", 0.315)

# Movement params
self.max_speed_linear = self.init_parameter("max_speed_linear", 2.0)
self.max_speed_angular = self.init_parameter("max_speed_angular", 1.0)
self.accel = int(self.init_parameter("acceleration", 1.0) * self.TICKS_PER_METER)
self.swap_motors = self.init_parameter("swap_motors", False)
self.stop_if_idle = self.init_parameter("stop_if_idle", True)
self.idle_timeout = self.init_parameter("idle_timeout", 1.0)

# Roboclaw params
self.custom_pid = self.init_parameter("custom_pid", True)
self.P = self.init_parameter("p_constant", 3.0)
self.I = self.init_parameter("i_constant", 0.42)
self.D = self.init_parameter("d_constant", 0.0)
self.qpps = self.init_parameter("qpps", 6000)

# NEW: encoder debug prints (helps detect reversed/dead encoder)
self.debug_encoders = self.init_parameter("debug_encoders", True)
self.debug_encoder_rate_hz = self.init_parameter("debug_encoder_rate_hz", 2.0) # print ~2 times/sec

# NEW: optionally disable dynamic PID writes (helps rule out instability)
self.enable_dynamic_params = self.init_parameter("enable_dynamic_params", False)
self.dynamic_params_period = self.init_parameter("dynamic_params_period", 1.0)

# Publishing params
self.ODOM_RATE = self.init_parameter("odom_rate", 10)
self.ELEC_RATE = self.init_parameter("elec_rate", 1)
self.PUBLISH_ODOM = self.init_parameter("publish_odom", True)
self.PUBLISH_ENCODERS = self.init_parameter("publish_encoders", True)
self.PUBLISH_ELEC = self.init_parameter("publish_elec", True)
self.PUBLISH_TF = self.init_parameter("publish_tf", False)

# IO params
self.S3Mode = self.init_parameter("S3_mode", 0x00)
self.S4Mode = self.init_parameter("S4_mode", 0x00)
self.S5Mode = self.init_parameter("S5_mode", 0x00)

# Voltage limit params (RoboClaw EEPROM settings)
self.min_main_voltage = self.init_parameter("min_main_voltage", 5.0)
self.max_main_voltage = self.init_parameter("max_main_voltage", 24.0)
self.min_logic_voltage = self.init_parameter("min_logic_voltage", 5.0)
self.max_logic_voltage = self.init_parameter("max_logic_voltage", 24.0)
self.automax_main_offset_voltage = self.init_parameter("automax_main_offset_voltage", 2.0)

# Optional 2nd RoboClaw
self.MIRROR_SECOND = self.init_parameter("mirror_second_board", False)
self.DEV2 = self.init_parameter("dev2", "")
self.ADDRESS2 = self.init_parameter("address2", self.ADDRESS)

# NEW: cmd_vel topic (so we can subscribe to twist_mux output)
self.cmd_vel_topic = self.init_parameter("cmd_vel_topic", "/twist_mux/cmd_vel")
 

def init_parameter(self, name, default):
"""Initialize a parameter and log it"""
value = self.declare_parameter(name, default).value
self.get_logger().info(f" {name}: {value}")
return value

def update_parameters(self):
"""
OPTIONAL dynamic update.
NOTE: Writing PID settings repeatedly while driving can cause weirdness on some setups.
That's why enable_dynamic_params defaults to False.
"""
self.max_speed_linear = self.get_parameter("max_speed_linear").value
self.max_speed_angular = self.get_parameter("max_speed_angular").value
self.stop_if_idle = self.get_parameter("stop_if_idle").value
self.idle_timeout = self.get_parameter("idle_timeout").value

self.custom_pid = self.get_parameter("custom_pid").value
self.P = self.get_parameter("p_constant").value
self.I = self.get_parameter("i_constant").value
self.D = self.get_parameter("d_constant").value
self.qpps = self.get_parameter("qpps").value
self.configure_device()

def init_device(self, dev_name: str, address: int, baud_rate: int):
"""Initialize the Roboclaw device and connect to it"""
try:
self.get_logger().info(f"Connecting to device {dev_name} with address {str(address)}")
driver = Roboclaw(dev_name, baud_rate, address)
driver.open()
self.get_logger().info("Connected!")
except Exception as e:
self.get_logger().fatal("Could not connect to device.")
self.get_logger().debug(e)
self.shutdown("Could not connect to Roboclaw")

try:
_, version = driver.ReadVersion()
self.get_logger().info(f"Roboclaw version: {str(version)}")
except Exception as e:
self.get_logger().warn("Problem getting Roboclaw version")
self.get_logger().debug(e)

return driver

def reset_device(self):
"""Reset the roboclaw speed and encoder counters"""
self.stop_motors()
try:
self.driver.ResetEncoders()
except OSError as e:
self.get_logger().warn(f"ResetEncoders OSError: {str(e)}")

if self.driver2 is not None:
try:
self.driver2.ResetEncoders()
except OSError as e:
self.get_logger().warn(f"ResetEncoders OSError (driver2): {str(e)}")

def configure_device(self):
"""Configure both boards: timeout, PID, pin modes."""
def _cfg(drv):
drv.SetSerialTimeout(int(self.serial_timeout * 10))
if self.custom_pid:
drv.SetM1VelocityPID(self.P, self.I, self.D, self.qpps)
drv.SetM2VelocityPID(self.P, self.I, self.D, self.qpps)
drv.SetPinFunctions(self.S3Mode, self.S4Mode, self.S5Mode)

_cfg(self.driver)
if self.driver2 is not None:
_cfg(self.driver2)

def create_subscribers(self):
"""Create subscribers for the node"""
self.cmd_vel_sub = self.create_subscription(
Twist,
str(self.cmd_vel_topic),
self.cmd_vel_callback,
10,
)
self.get_logger().info(f"Subscribed to cmd_vel_topic: {self.cmd_vel_topic}")

def create_wrappers(self):
"""Create the encoder and electrical wrappers (NOTE: wrappers use driver1 only)"""
self.encoder_wrapper = EncoderWrapper(
self,
self.driver,
self.TICKS_PER_METER,
self.TICKS_PER_ROTATION,
self.BASE_WIDTH,
self.PUBLISH_ODOM,
self.PUBLISH_ENCODERS,
self.PUBLISH_TF,
self.swap_motors,
)
self.electrical_wrapper = ElectricalWrapper(self, self.driver, self.PUBLISH_ELEC, self.swap_motors)

def create_timers(self):
"""Create timers for the node"""
if self.ODOM_RATE:
self.odom_timer = self.create_timer(1.0 / self.ODOM_RATE, self.encoder_wrapper.update_and_publish)
if self.ELEC_RATE:
self.elec_timer = self.create_timer(1.0 / self.ELEC_RATE, self.electrical_wrapper.update_and_publish)

self.idle_timer = self.create_timer(1 / 30, self.idle_callback)

# NEW: make dynamic param updates optional (default False)
if bool(self.enable_dynamic_params):
period = float(self.dynamic_params_period)
if period <= 0:
period = 1.0
self.dynamic_params_timer = self.create_timer(period, self.update_parameters)

def idle_callback(self):
"""Stop the robot if no commands are received for 'idle_timeout' seconds"""
now = self.get_clock().now().nanoseconds
if self.stop_if_idle and now - self.last_cmd_timestamp > self.idle_timeout * 1e9:
self.get_logger().debug(
f"Did not get command for {self.idle_timeout} second, stopping"
)
self.stop_motors()

# -----------------------------
# NEW: debug encoder helper
# -----------------------------
def _maybe_log_encoders(self):
if not self.debug_encoders:
return
rate_hz = float(self.debug_encoder_rate_hz) if self.debug_encoder_rate_hz else 0.0
if rate_hz <= 0:
return

now_ns = self.get_clock().now().nanoseconds
min_period_ns = int(1e9 / rate_hz)

if now_ns - self._last_enc_log_ns < min_period_ns:
return

self._last_enc_log_ns = now_ns

try:
e1 = self.driver.ReadEncM1()
e2 = self.driver.ReadEncM2()
self.get_logger().info(f" board1 M1={e1} M2={e2}")
except Exception as e:
self.get_logger().warn(f" board1 read failed: {e}")

if self.driver2 is not None:
try:
e3 = self.driver2.ReadEncM1()
e4 = self.driver2.ReadEncM2()
self.get_logger().info(f" board2 M1={e3} M2={e4}")
except Exception as e:
self.get_logger().warn(f" board2 read failed: {e}")

def cmd_vel_callback(self, twist):
"""Callback for /cmd_vel topic, move the robot according to the Twist message"""

# Limit the speed
linear_x = min(max(twist.linear.x, -self.max_speed_linear), self.max_speed_linear)
angular_z = min(max(twist.angular.z, -self.max_speed_angular), self.max_speed_angular)

if abs(linear_x) < 0.01 and abs(angular_z) < 0.01:
self.stop_motors()
return
 
# Convert to motor speeds (ticks/s)
right_speed = int((linear_x + angular_z * self.BASE_WIDTH / 2) * self.TICKS_PER_METER)
left_speed = int((linear_x - angular_z * self.BASE_WIDTH / 2) * self.TICKS_PER_METER)

self.get_logger().debug(
f"Sending command -> left: {left_speed}, right: {right_speed} (ticks/sec)"
)

# Send the command
try:
# Your preferred mapping:
# each board: M1=left, M2=right
if self.swap_motors:
# swap only swaps left/right assignment
self.driver.SpeedAccelM1M2(self.accel, right_speed, left_speed)
if self.driver2 is not None:
self.driver2.SpeedAccelM1M2(self.accel, right_speed, left_speed)
else:
self.driver.SpeedAccelM1M2(self.accel, left_speed, right_speed)
if self.driver2 is not None:
self.driver2.SpeedAccelM1M2(self.accel, left_speed, right_speed)

self.last_cmd_timestamp = self.get_clock().now().nanoseconds

# NEW: encoder debug prints (throttled)
self._maybe_log_encoders()

except OSError as e:
# NEW: if command fails, STOP BOTH immediately (prevents "one motor keeps running")
self.get_logger().warn(f"SpeedAccelM1M2 OSError: {str(e)} -> stopping motors")
self.stop_motors()

except Exception as e:
# Catch-all safety stop
self.get_logger().warn(f"SpeedAccelM1M2 unexpected error: {e} -> stopping motors")
self.stop_motors()

def diagnostics(self, stat):
"""Read the error status of the Roboclaw and report it as diagnostics"""
stat.summary(diagnostic_updater.DiagnosticStatus.STALE, "No data")

try:
status = self.driver.ReadError()
statuses = u.decipher_rclaw_status(status)
for state, message in statuses:
stat.summary(state, message)
self.get_logger().debug("Got unit status: " + str(hex(status)))
except Exception as e:
self.get_logger().warn("Diagnostics error: " + str(e))
self.get_logger().debug(e)
return
return stat

def stop_motors(self):
"""Stop motors aggressively; handle each board independently."""
# Board 1
try:
# Immediate stop if supported by your library
self.driver.SpeedM1M2(0, 0)
except Exception:
pass

try:
# Ramp stop fallback
self.driver.SpeedAccelM1M2(self.accel, 0, 0)
except Exception as e:
self.get_logger().warn(f"Stop failed on driver1: {e}")

# Board 2
if self.driver2 is not None:
try:
self.driver2.SpeedM1M2(0, 0)
except Exception:
pass

try:
self.driver2.SpeedAccelM1M2(self.accel, 0, 0)
except Exception as e:
self.get_logger().warn(f"Stop failed on driver2: {e}")

self.last_cmd_timestamp = self.get_clock().now().nanoseconds

def shutdown(self, str_msg):
"""Shutdown the node"""
self.get_logger().info("Shutting down: " + str_msg)
self.stop_motors()
self.stop_motors() # Call twice to make sure


def main(args=None):
rclpy.init(args=args)

roboclaw_node = RoboclawNode()
rclpy.spin(roboclaw_node)
roboclaw_node.destroy_node()
rclpy.shutdown()


if __name__ == "__main__":
main()
]]> eunsun https://robartx.tech/community/robocall/code/#post-36 RE: code https://robartx.tech/community/robocall/code/#post-35 Fri, 27 Feb 2026 19:29:11 +0000 encoder_wrapper.py

 

f
from math import cos, pi, sin, fmod

from tcr_roboclaw import Roboclaw
from rclpy.node import Node
from geometry_msgs.msg import Quaternion, TransformStamped
from nav_msgs.msg import Odometry
from roboclaw_msgs.msg import EncoderState
from tf_transformations import quaternion_from_euler
from tf2_ros import TransformBroadcaster

COUNTER_MAX = 2**32


class EncoderWrapper:

def __init__(
self,
node: Node,
driver: Roboclaw,
ticks_per_meter: float,
ticks_per_rotation: float,
base_width: float,
pub_odom: bool,
pub_encoders: bool,
pub_tf: bool,
swap_motors: bool,
):
"""Encoder Wrapper

Args:
node (rclpy.node.Node): ROS 2 node
driver (tcr_roboclaw.Roboclaw): Roboclaw driver
ticks_per_meter (float): Encoder ticks per meter
ticks_per_rotation (float): Encoder ticks per rotation
base_width (float): Distance between the two wheels
pub_odom (bool): Publish odometry data
pub_encoders (bool): Publish encoder data
pub_tf (bool): Publish the transform from odom to base_link
swap_motors (bool): Swap the left and right motors
"""
self.TICKS_PER_METER = ticks_per_meter
self.TICKS_PER_ROTATION = ticks_per_rotation
self.BASE_WIDTH = base_width
self.driver = driver

# Node parameters
self.node = node
self.clock = self.node.get_clock()
self.logger = self.node.get_logger()
self.PUB_ODOM = pub_odom
self.PUB_ENCODERS = pub_encoders
self.PUB_TF = pub_tf
self.swap_motors = swap_motors

# Encoder data
self.timestamp = self.clock.now()
self.left_ticks =
self.right_ticks =
self.left_velocity = 0
self.right_velocity = 0
self.poll_encoders()

# Odometry data
self.pose_x = 0
self.pose_y = 0
self.pose_theta = 0.0
self.vel_x = 0.0
self.vel_theta = 0.0

# Publishers
# TODO: Topics should be parameters
self.left_encoder_pub = self.node.create_publisher(
EncoderState,
"/motors/left/encoder",
10,
)
self.right_encoder_pub = self.node.create_publisher(
EncoderState,
"/motors/right/encoder",
10,
)
self.odom_pub = self.node.create_publisher(Odometry, "/motors/odometry", 10)
self.tf_broadcaster = TransformBroadcaster(self.node)

@staticmethod
def normalize_angle(angle):
return fmod(angle + pi, 2.0 * pi) - pi

def update_and_publish(self):
"""Update the odometry and publish the data"""

if self.poll_encoders():
self.update_odometry()
if self.PUB_ODOM:
self.publish_odometry()
if self.PUB_ENCODERS:
self.publish_encoder_data()
if self.PUB_TF:
self.broadcast_tf()
else:
self.logger.warn("Failed to poll encoders.")

def poll_encoders(self):
"""Poll the encoder data from the hardware"""

status_enc, status_speed = 0, 0

try:
self.timestamp = self.clock.now()
status_enc, ticks1, ticks2 = self.driver.GetEncoderCounters()
status_speed, speed1, speed2 = self.driver.GetMotorAverageSpeeds()
except Exception as e:
self.logger.warn(f"Read encoders error: {str(e)}")

# Update the encoder ticks
if status_enc == 1:
if self.swap_motors:
self.right_ticks = [self.right_ticks, ticks1]
self.left_ticks = [self.left_ticks, ticks2]
else:
self.right_ticks = [self.right_ticks, ticks2]
self.left_ticks = [self.left_ticks, ticks1]

# Update the motor velocities
if status_speed == 1:
if self.swap_motors:
self.right_velocity = speed1
self.left_velocity = speed2
else:
self.right_velocity = speed2
self.left_velocity = speed1

return status_enc and status_speed

def update_odometry(self):
"""Update the odometry estimation"""

# Compute the ticks difference (handle overflow)
delta_ticks_left = self.left_ticks - self.left_ticks
delta_ticks_right = self.right_ticks - self.right_ticks

# Handle overflow and underflow
if delta_ticks_left < -COUNTER_MAX / 2:
delta_ticks_left += COUNTER_MAX
elif delta_ticks_left > COUNTER_MAX / 2:
delta_ticks_left -= COUNTER_MAX
if delta_ticks_right < -COUNTER_MAX / 2:
delta_ticks_right += COUNTER_MAX
elif delta_ticks_right > COUNTER_MAX / 2:
delta_ticks_right -= COUNTER_MAX

# Compute the traveled distance
dist_left = delta_ticks_left / self.TICKS_PER_METER
dist_right = delta_ticks_right / self.TICKS_PER_METER
dist_center = (dist_right + dist_left) / 2.0

# Compute the displacement
d_theta = (dist_right - dist_left) / self.BASE_WIDTH
d_x = dist_center * cos(self.pose_theta + d_theta / 2.0)
d_y = dist_center * sin(self.pose_theta + d_theta / 2.0)

# Update the pose
self.pose_x += d_x
self.pose_y += d_y
self.pose_theta = self.normalize_angle(self.pose_theta + d_theta)

# Compute the linear and angular velocities
vel_left = self.left_velocity / self.TICKS_PER_METER
vel_right = self.right_velocity / self.TICKS_PER_METER
self.vel_x = (vel_left + vel_right) / 2.0
self.vel_theta = (vel_right - vel_left) / self.BASE_WIDTH

def publish_odometry(self):
"""Publish odometry data to the ROS network"""

odom = Odometry()
odom.header.stamp = self.timestamp.to_msg()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"

odom.pose.pose.position.x = self.pose_x
odom.pose.pose.position.y = self.pose_y
odom.pose.pose.position.z = 0.0

quat = quaternion_from_euler(0, 0, self.pose_theta)
odom.pose.pose.orientation = Quaternion(
x=quat,
y=quat,
z=quat,
w=quat,
)

odom.twist.twist.linear.x = self.vel_x
odom.twist.twist.linear.y = 0.0
odom.twist.twist.angular.z = self.vel_theta

self.odom_pub.publish(odom)

def publish_encoder_data(self):
"""Publish the encoder data to the ROS network"""

encoder_state = EncoderState()
encoder_state.header.stamp = self.timestamp.to_msg()
encoder_state.header.frame_id = "base_link"
encoder_state.ticks_per_rotation = float(self.TICKS_PER_ROTATION)
encoder_state.ticks_per_meter = float(self.TICKS_PER_METER)

encoder_state.position = self.left_ticks
encoder_state.velocity = float(self.left_velocity)
self.left_encoder_pub.publish(encoder_state)

encoder_state.position = self.right_ticks
encoder_state.velocity = float(self.right_velocity)
self.right_encoder_pub.publish(encoder_state)

def broadcast_tf(self):
"""Broadcast the transform from odom to base_link"""

t = TransformStamped()
t.header.stamp = self.timestamp.to_msg()
t.header.frame_id = "odom"
t.child_frame_id = "base_link"

t.transform.translation.x = self.pose_x
t.transform.translation.y = self.pose_y
t.transform.translation.z = 0.0

quat = quaternion_from_euler(0, 0, self.pose_theta)
t.transform.rotation = Quaternion(x=quat, y=quat, z=quat, w=quat)

self.tf_broadcaster.sendTransform(t)
]]> eunsun https://robartx.tech/community/robocall/code/#post-35 code https://robartx.tech/community/robocall/code/#post-34 Fri, 27 Feb 2026 19:26:46 +0000
from roboclaw_msgs.msg import MotorState
from tcr_roboclaw import Roboclaw
from rclpy.node import Node
from copy import deepcopy


class ElectricalWrapper:

def __init__(self, node: Node, driver: Roboclaw, pub_elec: bool, swap_motors: bool):
"""Electrical Wrapper

Args:
node (rclpy.node.Node): ROS 2 node
driver (tcr_roboclaw.Roboclaw): RoboClaw driver
pub_elec (bool): Publish electrical data
swap_motors (bool): Swap the left and right motors
"""

# Node parameters
self.driver = driver
self.node = node
self.clock = self.node.get_clock()
self.logger = self.node.get_logger()
self.PUB_ELEC = pub_elec
self.swap_motors = swap_motors

# Electrical data
self.timestamp = self.clock.now()
self.voltage = 0.0
self.currents =
self.temperature = 0.0
self.duty_cycles =
self.poll_electrical_data()

# Publishers
self.left_elec_pub = self.node.create_publisher(
MotorState,
"/motors/left/electrical",
1,
)
self.right_elec_pub = self.node.create_publisher(
MotorState,
"/motors/right/electrical",
1,
)

def update_and_publish(self):
"""Update and publish electrical data"""

if self.poll_electrical_data():
if self.PUB_ELEC:
self.publish_elec()
else:
self.logger.warn("Failed to poll electrical data.")

def poll_electrical_data(self):
"""Poll electrical data from the Roboclaw"""

status1, status2, status3, status4 = 0, 0, 0, 0

try:
self.timestamp = self.clock.now()
status1, *currents = self.driver.ReadCurrents()
status2, voltage = self.driver.ReadMainBatteryVoltage()
status3, *pwms = self.driver.ReadPWMs()
status4, temp = self.driver.ReadTemp()
except Exception as e:
self.logger.warn(f"Read electrical data error: {str(e)}")

if status1 == 1:
self.currents =
if status2 == 1:
self.voltage = voltage / 10
if status3 == 1:
self.duty_cycles =
if status4 == 1:
self.temperature = temp / 10

return status1 and status2 and status3 and status4

def publish_elec(self):
"""Publish electrical data in two MotorState messages"""

motor_state = MotorState()
motor_state.header.stamp = self.timestamp.to_msg()
motor_state.header.frame_id = "base_link"
motor_state.voltage = self.voltage
motor_state.temperature = self.temperature

motor_state1, motor_state2 = deepcopy(motor_state), deepcopy(motor_state)

motor_state1.current = self.currents
motor_state1.duty = self.duty_cycles
motor_state2.current = self.currents
motor_state2.duty = self.duty_cycles

if self.swap_motors:
self.right_elec_pub.publish(motor_state2)
self.left_elec_pub.publish(motor_state1)
else:
self.right_elec_pub.publish(motor_state1)
self.left_elec_pub.publish(motor_state2)
]]> eunsun https://robartx.tech/community/robocall/code/#post-34 RE: Get ROS2 Jazzy docker image on Jetson https://robartx.tech/community/docker-image-ros2-jazzy-on-jetson/get-ros2-jazzy/#post-31 Fri, 27 Feb 2026 18:51:02 +0000 In Local terminal 

 

xhost +local:root

 

Docker image start

 

sudo docker start -ai robot_10_jazzy

 

sudo docker exec -it robot_10_jazzy bash

 

source /opt/ros/jazzy/install/setup.bash

source /ws_big_robot/ws_big_robot/install/setup.bash

 

]]> eunsun https://robartx.tech/community/docker-image-ros2-jazzy-on-jetson/get-ros2-jazzy/#post-31 Connecting LIDAR while not disconnecting the Roboclaw https://robartx.tech/community/robocall/connecting-lidar-while-not-disconnecting-the-roboclaw/#post-30 Fri, 20 Feb 2026 19:05:35 +0000 When we first connected the LIDAR to Jetson through USB-A (serial), the motors stopped moving and teleop stopped working because the connection of the LIDAR suspended the connection of the other USBs. The connection activity can be seen by doing the following commands:

ls -l /dev/ttyUSB* /dev/ttyACM* /dev/serial/by-id/ 2>/dev/null
dmesg | tail -n 80

 

The problem is fixed by simply disabling the USB autosuspend on Jetson, by running the following command:

sudo bash -c 'echo -1 > /sys/module/usbcore/parameters/autosuspend'
]]> qiancai https://robartx.tech/community/robocall/connecting-lidar-while-not-disconnecting-the-roboclaw/#post-30 Battery https://robartx.tech/community/main-forum/battery/#post-29 Sat, 14 Feb 2026 02:55:19 +0000 Battery Size Estimation Calculations:
Battery specs depend on a few factors including power consumption of the robot, runtime of the robot, and continuous current draw of the robot. Based on preliminary estimates, power consumption seems to lie around 150W and current draw seems to be around 8A-12A (mostly dependent on the current draw of the Jetson Orin Nano). We aim to run the robot for around 5 hours, however, this increases the amp-hour capacity we need for the battery. Below are some calculations showing how runtime affects battery capacity as well as links to an excel spreadsheet and google doc containing everything I've done so far:

Runtime (hours) Watt-Hours (Wh) Amp-Hours (Ah)
2 268.8 22.4
3 403.2 33.6
4 537.6 44.8
5 672 56

As you can see, we would need around 50Ah to run the robot for 5 hours continuously, and only 33Ah to run it for 3 hours. It is important to note that these numbers assume the robot is consuming 150W continuously which may not be the case.

Battery Constraints:

The height of the battery should be less than 5, 1/2 to be placed in the bottom compartment of the robot. If the battery is any larger, the frame design will have to be changed to accommodate the larger battery size. Width and length of the battery do not seem to matter (as far as I know!). This space constraint incentivizes us to use multiple smaller batteries and hook them up in parallel rather than using one larger battery, however, the capacity of these smaller batteries may not be enough to effectively support the projects needs. Below are some options I currently have in mind:

Option 1) Hook up Four 12V 8Ah batteries from lab (Model # SLA1079)

This would allow the batteries to fit under the frame while maintaining higher Ah capacity. This matches the project needs pretty well and uses batteries that are already owned, however, there are a few problems with it which I've listed below:

      a) With four batteries in parallel we can achieve 36Ah of capacity which can power the robot for only 3 hours continuously
          instead of 5.

      b) The current draw of the robot may be too high for the battery to supply continuously at a healthy level. The SLA1079
          datasheet provides us with a table that relates battery runtime and continuous current usage over time. It shows that
          continuous current should be lower if the battery is used for longer periods of time. This is to ensure battery health. Basically,
          if you run the battery for longer, you shouldn't draw as much current from it continuously, otherwise you hurt the battery over
          time. This table can be found in the google doc provided (I'd attach an image but I don't think the forum allows me to do that)

Option 2) Hook up Six 12V 8Ah batteries (SLA1079)

Same as option 1, but we increase capacity and current draw which solve problems a) and b). This could work, however, I'm not sure if we can safely connect 6 batteries, it may cause current draw issues. I will look into this.

Option 3) Purchase a larger battery

We could purchase a larger battery (40-60Ah battery to support 5+ hours of continuous use). This would increase the runtime of the robot as well as ensure the current draw of the robot can be easily managed. There are a few issues with this method though:

a) Increases the weight of the robot, potentially reducing mobility

b) The battery would be too large to fit inside the hull, so the hull would have to be modified to accommodate the larger battery.

Other Notes:

This is all the info I have now, but I'll keep doing research and figure out if hooking up 6 12V 8Ah batteries is safe, as well as looking into the current draw of both small and large batteries to see if they can handle the continuous current draw of our system.

Here are links to the excel spreadsheet and google doc with all my work. This information can also be found in the google drive.

Excel:
https://docs.google.com/spreadsheets/d/1c61AJ6dTo-TDHi3su-V8-ZS31RmROvyz_vDzj9C5zlU/edit?usp=sharing 

Google Doc:
https://docs.google.com/spreadsheets/d/1c61AJ6dTo-TDHi3su-V8-ZS31RmROvyz_vDzj9C5zlU/edit?usp=sharing 

]]> cjheller https://robartx.tech/community/main-forum/battery/#post-29 Using SLAM Toolbox + Nav2 with RPLIDAR A1 https://robartx.tech/community/lidar/using-slam-toolbox-nav2-with-rplidar-a1/#post-28 Fri, 13 Feb 2026 07:15:25 +0000 The following must be running correctly:

  • RPLIDAR publishes /scan (LaserScan)
  • TF tree is complete: map --> odom -->  base_link --> laser
  • SLAM toolbox publishes /map and map --> odom
  • Nav2 runs without AMCL + without map_server while you are mapping
    • Nav2 expects SLAM to provide those

Bringup: 

1) Install packages (Jazzy)

sudo apt update
sudo apt install -y \
  ros-jazzy-navigation2 ros-jazzy-nav2-bringup \
  ros-jazzy-slam-toolbox \
  ros-jazzy-tf2-ros

2) Run RPLIDAR A1 driver (rplidar_ros)

ros2 launch rplidar_ros view_rplidar_a1_launch.py

Confirm the scan topic (in another terminal)

ros2 topic list | grep scan
ros2 topic echo /scan --once

header.frame_id has to be laser or laser_frame. This frame must match the TF.

3) Provide the TF from base_link --> laser

If not already published with URDF + robot_state_publisher, publish a static transform:

Ex; laser is 0.20m forward, 0.10m up from base_link , no rotation 

ros2 run tf2_ros static_transform_publisher \

  0.20 0.0 0.10 0 0 0 base_link laser

Check:

ros2 run tf2_ros tf2_echo base_link laser

4) Check odometry (odom --> base_link)

Odom comes from wheel encoders.

ros2 topic echo /odom --once

ros2 run tf2_ros tf2_echo odom base_link

5) Start SLAM toolbox (online async)

6) Start Nav2 (navigation only, no AMCL/map_server)

7) Start RViz

8) Save map

]]> tanya https://robartx.tech/community/lidar/using-slam-toolbox-nav2-with-rplidar-a1/#post-28 RE: Autonomous Exploration https://robartx.tech/community/slam-nav2/autonomous-exploration/#post-27 Fri, 13 Feb 2026 06:55:12 +0000 If m-explore doesn't work, try installing nav2_wfd explore.

  • clone it
  • build it
  • run: ros2 run nav2_wfd explore

It computes frontier centroids from the OccupancyGrid and invokes Nav2 waypoint follower. 

Install

cd ~/ws_big_robot/src

git clone https://github.com/SeanReg/nav2_wavefront_frontier_exploration.git

cd ~/ws_big_robot

rosdep update

rosdep install -i --from-path src --rosdistro jazzy -y

colcon build --symlink-install

source install/setup.bash

Run

ros2 run nav2_wfd explore

]]> tanya https://robartx.tech/community/slam-nav2/autonomous-exploration/#post-27 Autonomous Exploration https://robartx.tech/community/slam-nav2/autonomous-exploration/#post-26 Fri, 13 Feb 2026 06:53:11 +0000 What the exploration node does:

Exploration is not part of Nav2. It is a goal generator:

- subscribes to /map (OccupancyGrid)

- finds frontiers (boundary between known free space and unknown space)

- picks a frontier goal pose

- calls Nav2's NavigateToPose (or waypoint follower) action so Nav2 drives the repeats until no frontiers remain

explorer never publishes /cmd_vel. Nav2 still owns motion.

 

Prereqs that must already work:

Before installing exploration, make sure:

  1. SLAM toolbox is publishing /map
  2. TF chain exists: map --> odom --> base_link --> laser
  3. Nav2 action server exists: navigate_to_pose

Check using:

  • ros2 topic echo /map --once

  • ros2 run tf2_ros tf2_echo map odom

  • ros2 action list | grep navigate_to_pose

 

Install:

1) Install dependencies (Jazzy):

sudo apt update

sudo apt install -y ros-jazzy-nav2-bringup ros-jazzy-slam-toolbox

2) Clone + build in your workspace

cd ~/ws_big_robot/src   # or whatever your colcon ws is

git clone https://github.com/robo-friends/m-explore-ros2.git

cd ~/ws_big_robot

rosdep update

rosdep install -i --from-path src --rosdistro jazzy -y

colcon build --symlink-install

source install/setup.bash

3) Confirm it installed

ros2 pkg list | grep explore

ros2 run explore_lite explore --help

4) How to run it manually (first test)

ros2 run explore_lite explore --ros-args --params-file \

  ~/ws_big_robot/src/m-explore-ros2/explore/config/params.yaml


It also supports stop/resume by publishing Bool to explore/resume.

 

Where to embed it in your system (your bringup XML):

You embed it in the same bringup launch after you start SLAM Toolbox + Nav2.

In your current bringup file, add this node near the bottom (after navigation include is fine):

<!-- Exploration node (frontier exploration) -->

<node pkg="explore_lite"

      exec="explore"

      name="explore"

      output="screen"

      args="--ros-args --params-file $(find-pkg-share big_robot_navigation)/params/explore.yaml">

  <param name="use_sim_time" value="$(var use_sim_time)"/>

 

  <!-- If you use namespaces, avoid absolute topics unless you really mean global -->

  <remap from="/tf" to="tf"/>

  <remap from="/tf_static" to="tf_static"/>

</node>

 

Important practical note (startup ordering):

explore_lite “starts right away” by default.
If it starts before Nav2 is fully active, it may fail goals.

Easy workaround: start it embedded, but immediately “pause” exploration and unpause after Nav2 is active:

# pause

ros2 topic pub /explore/resume std_msgs/msg/Bool "{data: false}" -1

 

# later, resume

ros2 topic pub /explore/resume std_msgs/msg/Bool "{data: true}" -1

 

Create params/explore.yaml (starter template):

Start simple; don’t over-tune until it moves.

# big_robot_navigation/params/explore.yaml

explore:

  ros__parameters:

    # Common knobs you’ll tune:

    # - how close is "goal reached"

    # - how long to wait before declaring stuck

    # - frontier size thresholds

    # These names vary slightly across explore implementations, so:

    # 1) start by copying the repo's explore/config/params.yaml

    # 2) move it here and edit

 

Best workflow:

  1. Copy the repo’s default params file into your package
  2. Edit locally
  3. Point the node at your copy

The repo explicitly shows where the default params live and how to run with them. 

 

Debugging when it “doesn’t move”

Most common causes:

  1. No /map (SLAM toolbox not running / wrong scan topic)
  2. TF broken (especially map→odom or odom→base_link)
  3. Nav2 not actually active (bt_navigator lifecycle not “active”)
  4. Topic mismatch (namespaces + you used /scan absolute in one place, relative in another)

Also: in RViz, add the frontiers marker if available:

  • m-explore-ros2 mentions a frontiers marker topic explore/frontiers.
]]> tanya https://robartx.tech/community/slam-nav2/autonomous-exploration/#post-26 RE: Roboclaw + ROS2 Setup https://robartx.tech/community/robocall/roboclaw-ros2-setup/#post-25 Thu, 12 Feb 2026 22:24:33 +0000 To use teleop keyboard (only key I and O works right now):

In terminal:

ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=/twist_mux/cmd_vel

Reference: see google doc for robotic mouse project: Teleop_keyboard_operation

]]> qiancai https://robartx.tech/community/robocall/roboclaw-ros2-setup/#post-25