From c4586f83a1171f3ff451de7ded970254e2cb8eaa Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Tue, 4 Nov 2025 21:40:31 +0100 Subject: [PATCH 1/2] improve serial hiwonder driver performance --- .devcontainer/docker-compose.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index cadb89e..40f509b 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -10,6 +10,8 @@ services: environment: - ROS_DOMAIN_ID=91 network_mode: "host" + group_add: + - "dialout" devices: - "/dev:/dev" privileged: true From 17a8f9865e7eab8eb8db620e309a03bf552fbe73 Mon Sep 17 00:00:00 2001 From: "Safeea, Mohammad" Date: Tue, 4 Nov 2025 21:41:32 +0100 Subject: [PATCH 2/2] improve serial hiwonder driver performance --- .../penta_hiwonder_actuators_node.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/penta_hiwonder_actuators/penta_hiwonder_actuators/penta_hiwonder_actuators_node.py b/penta_hiwonder_actuators/penta_hiwonder_actuators/penta_hiwonder_actuators_node.py index 25a6cb6..c16628e 100644 --- a/penta_hiwonder_actuators/penta_hiwonder_actuators/penta_hiwonder_actuators_node.py +++ b/penta_hiwonder_actuators/penta_hiwonder_actuators/penta_hiwonder_actuators_node.py @@ -84,8 +84,7 @@ def update_motors_callback(self): for i in range(self.joints_count): position_degree = self.actuator_setpoint_degree[i] actuation_range_degree = self.servo_actuation_range_degree[i] - ticks_span = self.servo_max_ticks[i] - self.servo_min_ticks[i] - position_ticks = self.servo_min_ticks[i] + float(position_degree * ticks_span) / actuation_range_degree + position_ticks = self.servo_min_ticks[i] + float(position_degree * self.ticks_spans[i]) / actuation_range_degree position_ticks = int(position_ticks) if (self.last_position_ticks[i] == position_ticks): continue # No change in position, skip @@ -95,6 +94,10 @@ def update_motors_callback(self): self.publish_ticks_message() def initialize_properties(self): + # Pre-compute conversion factors + self.deg_per_rad = 180.0 / math.pi + self.ticks_spans = [max_tick - min_tick for max_tick, min_tick in + zip(self.servo_max_ticks, self.servo_min_ticks)] self.index_cache = None self.joints_states_names = [] self.last_position_ticks = [-1] * self.joints_count # to track last sent position @@ -141,10 +144,9 @@ def on_joint_states_callback(self, msg): self.index_cache = None return # Compute actuator setpoints (vectorized-like) - deg_per_rad = 180.0 / math.pi for i in range(self.joints_count): q_rad = pos_list[self.index_cache[i]] # geometrical joint angle rads - setpoint_degree = self.dir[i] * (q_rad * deg_per_rad) + self.initial_joints_bias_degree[i] + setpoint_degree = self.dir[i] * (q_rad * self.deg_per_rad) + self.initial_joints_bias_degree[i] self.actuator_setpoint_degree[i] = self.clamp( setpoint_degree, i, 0.0, self.servo_actuation_range_degree[i] ) @@ -172,6 +174,7 @@ def send_serial_command(self, servo_id, cmd, params=None): packet.append(checksum) now = time.time() self.serial.write(bytearray(packet)) + self.serial.flush() # Ensure immediate transmission index = servo_id - 1 if self.actuator_update_stamps[index].last_serial_update < 0.0: self.actuator_update_stamps[index].last_serial_update = now @@ -204,7 +207,6 @@ def publish_actuators_setpoint(self): def publish_ticks_message(self): self.actuator_msg_ticks.header.stamp = self.get_clock().now().to_msg() - self.actuator_msg_ticks.name = self.joints_states_names self.actuator_msg_ticks.position = [float(tick) for tick in self.last_position_ticks] self.ticks_publisher_.publish(self.actuator_msg_ticks) self.motors_update_msg_hz.data = self.motor_update_hz @@ -303,4 +305,4 @@ def main(args=None): if __name__ == '__main__': - main() + main() \ No newline at end of file