Revert driving the tool-mount servo: it is a leftover, not a tool

src/tattotronix_description/config/servo_calibration.yaml

@@ -22,12 +22,9 @@
 # two will fight each other at full stall current. Check it with only one of
 # them connected.
 #
-# The tool's continuous-rotation SG90 is on channel 6. Its pulse sets a speed,
-# not an angle, so it is not a joint: the driver gives it a speed interface from
-# -1 to 1. stop_us is the pulse at which it stands still, which is a trim and
-# drifts, so the driver sends no pulse at all for a speed of zero; us_per_speed
-# is the pulse added for full speed. DECLARED like the rest: 1500 us to stop,
-# 500 us either side for full speed, the convention for a continuous servo.
+# The tool's continuous-rotation SG90 is on channel 6. It is listed so that the
+# wiring diagram, docs/scripts/wiring_diagram.py, draws it; the driver does not
+# command it yet.
 
 servos:
   joint_1: {channel: 0, zero_us: 1500.0, us_per_rad: 636.62, min_us: 500.0, max_us: 2500.0}
@@ -37,4 +34,4 @@ servos:
   joint_4: {channel: 4, zero_us: 1500.0, us_per_rad: 636.62, min_us: 500.0, max_us: 2500.0}
   joint_5: {channel: 5, zero_us: 1500.0, us_per_rad: 636.62, min_us: 500.0, max_us: 2500.0}
 
-tool: {channel: 6, stop_us: 1500.0, us_per_speed: 500.0, min_us: 1000.0, max_us: 2000.0}
+tool: {channel: 6}

src/tattotronix_description/urdf/tattotronix.ros2_control.xacro

@@ -86,25 +86,6 @@
       <xacro:controlled_joint name="joint_4" initial_position="0.0"/>
       <xacro:controlled_joint name="joint_5" initial_position="0.0"/>
 
-      <!-- The tool's continuous servo, on the real arm, once the calibration
-           gives its stop pulse. A speed rather than an angle, so a gpio
-           rather than a joint, and nothing robot_state_publisher needs. -->
-      <xacro:if value="${hardware == 'pca9685' and 'tool' in calibration and 'stop_us' in calibration['tool']}">
-        <xacro:property name="tool_servo" value="${calibration['tool']}"/>
-        <gpio name="tool">
-          <command_interface name="speed">
-            <param name="min">-1</param>
-            <param name="max">1</param>
-          </command_interface>
-          <state_interface name="speed"/>
-          <param name="channel">${tool_servo['channel']}</param>
-          <param name="stop_us">${tool_servo['stop_us']}</param>
-          <param name="us_per_speed">${tool_servo['us_per_speed']}</param>
-          <param name="min_us">${tool_servo['min_us']}</param>
-          <param name="max_us">${tool_servo['max_us']}</param>
-        </gpio>
-      </xacro:if>
-
     </ros2_control>
 
   </xacro:macro>

src/tattotronix_hardware/config/hardware.yaml

@@ -14,24 +14,9 @@ controller_manager:
     # There is no simulator, so no /clock to follow.
     use_sim_time: false
 
-    # The tool's motor, which only the real arm has: a speed from -1 to 1 on the
-    # driver's tool interface, forwarded as it comes. The drawing node sets it.
-    tool_controller:
-      type: forward_command_controller/ForwardCommandController
-
 joint_state_broadcaster:
   ros__parameters:
     use_sim_time: false
-    # The tool's speed is an interface but not a joint. Naming the joints keeps
-    # it out of /joint_states, which robot_state_publisher and MoveIt read as
-    # the arm's joints.
-    joints: [joint_1, joint_2, joint_3, joint_4, joint_5]
-    interfaces: [position, velocity]
-
-tool_controller:
-  ros__parameters:
-    joints: [tool]
-    interface_name: speed
 
 arm_controller:
   ros__parameters:

src/tattotronix_hardware/include/tattotronix_hardware/pca9685_servo_system.hpp

@@ -42,11 +42,6 @@ namespace tattotronix_hardware
 /// limits, the servo's pulse limits and the board's rounding - and the velocity
 /// is the rate that changes at. It is an honest echo of the command, not a
 /// measurement, and nothing downstream should read it as one.
-///
-/// A continuous-rotation servo, the tool's, is a gpio rather than a joint: its
-/// pulse sets a speed, not an angle, so it takes a speed from -1 to 1. A speed
-/// of zero sends no pulse at all, because the pulse at which such a servo
-/// stands still is a trim that drifts, and no pulse always stops it.
 class Pca9685ServoSystem : public hardware_interface::SystemInterface
 {
 public:
@@ -76,20 +71,6 @@ class Pca9685ServoSystem : public hardware_interface::SystemInterface
   const RecordingBus * recording() const;
 
 private:
-  /// A continuous servo. Its ServoChannel maps speed rather than angle:
-  /// zero_us is the pulse that stops it, us_per_rad the pulse per unit speed.
-  struct Tool
-  {
-    std::string name;
-    ServoChannel servo;
-    int sent = -1;  // the count last sent; kOff once its channel is off
-    double command = 0.0;
-    double speed = 0.0;
-    bool has_state = false;
-  };
-
-  static constexpr int kOff = -2;
-
   struct Joint
   {
     std::string name;
@@ -105,11 +86,9 @@ class Pca9685ServoSystem : public hardware_interface::SystemInterface
   };
 
   void send(Joint & joint);
-  void send(Tool & tool);
   void stop();
 
   std::vector<Joint> joints_;
-  std::vector<Tool> tools_;
   std::string device_ = "/dev/i2c-1";
   int address_ = 0x40;
   double frame_hz_ = 50.0;

src/tattotronix_hardware/launch/arm.launch.py

@@ -120,16 +120,6 @@ def generate_launch_description():
         launch_arguments={'spawn_timeout': '30'}.items(),
     )
 
-    # The tool's motor has a controller only here: the simulation has no tool
-    # interface to give it.
-    tool_controller = Node(
-        package='controller_manager',
-        executable='spawner',
-        arguments=['tool_controller', '--controller-manager', '/controller_manager',
-                   '--controller-manager-timeout', '30'],
-        output='screen',
-    )
-
     # The node waits for the controller's action server itself, so it can start
     # with everything else.
     draw = Node(
@@ -146,4 +136,4 @@ def generate_launch_description():
     )
 
     return LaunchDescription(
-        arguments + [robot_state_publisher, control_node, controllers, tool_controller, draw])
+        arguments + [robot_state_publisher, control_node, controllers, draw])

src/tattotronix_hardware/package.xml

@@ -15,7 +15,6 @@
   <depend>rclcpp_lifecycle</depend>
 
   <exec_depend>controller_manager</exec_depend>
-  <exec_depend>forward_command_controller</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>

src/tattotronix_hardware/src/pca9685_servo_system.cpp

@@ -35,8 +35,6 @@ namespace
 
 using Params = std::unordered_map<std::string, std::string>;
 
-constexpr char kSpeed[] = "speed";
-
 rclcpp::Logger logger()
 {
   return rclcpp::get_logger("Pca9685ServoSystem");
@@ -197,57 +195,6 @@ Pca9685ServoSystem::CallbackReturn Pca9685ServoSystem::on_init(
     joint.previous = joint.position;
     joints_.push_back(joint);
   }
-
-  tools_.clear();
-  tools_.reserve(info.gpios.size());
-  for (const auto & component : info.gpios) {
-    Tool tool;
-    tool.name = component.name;
-    const char * name = tool.name.c_str();
-    if (component.command_interfaces.size() != 1 ||
-      component.command_interfaces[0].name != kSpeed)
-    {
-      RCLCPP_ERROR(
-        logger(), "'%s' must have one command interface, speed: a continuous servo "
-        "takes nothing else", name);
-      return CallbackReturn::ERROR;
-    }
-    for (const auto & state : component.state_interfaces) {
-      if (state.name != kSpeed) {
-        RCLCPP_ERROR(
-          logger(), "'%s' asks for a '%s' state; a continuous servo has only its speed",
-          name, state.name.c_str());
-        return CallbackReturn::ERROR;
-      }
-      tool.has_state = true;
-    }
-    if (!component.parameters.count("channel")) {
-      RCLCPP_ERROR(logger(), "'%s' names no PCA9685 channel", name);
-      return CallbackReturn::ERROR;
-    }
-    double channel = -1.0;
-    if (!optional_number(component.parameters, "channel", channel) ||
-      !optional_number(component.parameters, "stop_us", tool.servo.zero_us) ||
-      !optional_number(component.parameters, "us_per_speed", tool.servo.us_per_rad) ||
-      !optional_number(component.parameters, "min_us", tool.servo.min_us) ||
-      !optional_number(component.parameters, "max_us", tool.servo.max_us))
-    {
-      RCLCPP_ERROR(logger(), "'%s' has a servo parameter that is not a number", name);
-      return CallbackReturn::ERROR;
-    }
-    tool.servo.channel = static_cast<int>(channel);
-    if (*tool.servo.problem()) {
-      RCLCPP_ERROR(logger(), "'%s': %s", name, tool.servo.problem());
-      return CallbackReturn::ERROR;
-    }
-    if (std::find(used.begin(), used.end(), tool.servo.channel) != used.end()) {
-      RCLCPP_ERROR(
-        logger(), "'%s': channel %d already drives another servo", name, tool.servo.channel);
-      return CallbackReturn::ERROR;
-    }
-    used.push_back(tool.servo.channel);
-    tools_.push_back(tool);
-  }
   return CallbackReturn::SUCCESS;
 }
 
@@ -285,11 +232,6 @@ Pca9685ServoSystem::CallbackReturn Pca9685ServoSystem::on_activate(
       std::fill(joint.sent.begin(), joint.sent.end(), -1);
       send(joint);
     }
-    for (auto & tool : tools_) {
-      tool.command = 0.0;
-      tool.sent = -1;
-      send(tool);
-    }
   } catch (const std::exception & error) {
     RCLCPP_ERROR(logger(), "cannot command the servos: %s", error.what());
     return CallbackReturn::ERROR;
@@ -331,11 +273,6 @@ std::vector<hardware_interface::StateInterface> Pca9685ServoSystem::export_state
       interfaces.emplace_back(joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity);
     }
   }
-  for (auto & tool : tools_) {
-    if (tool.has_state) {
-      interfaces.emplace_back(tool.name, kSpeed, &tool.speed);
-    }
-  }
   return interfaces;
 }
 
@@ -346,9 +283,6 @@ Pca9685ServoSystem::export_command_interfaces()
   for (auto & joint : joints_) {
     interfaces.emplace_back(joint.name, hardware_interface::HW_IF_POSITION, &joint.command);
   }
-  for (auto & tool : tools_) {
-    interfaces.emplace_back(tool.name, kSpeed, &tool.command);
-  }
   return interfaces;
 }
 
@@ -375,9 +309,6 @@ hardware_interface::return_type Pca9685ServoSystem::write(
         send(joint);
       }
     }
-    for (auto & tool : tools_) {
-      send(tool);
-    }
   } catch (const std::exception & error) {
     RCLCPP_ERROR(logger(), "lost the PCA9685: %s", error.what());
     return hardware_interface::return_type::ERROR;
@@ -406,25 +337,6 @@ void Pca9685ServoSystem::send(Joint & joint)
   joint.position = joint.servos[0].angle(joint.sent[0] * board_->count_us());
 }
 
-void Pca9685ServoSystem::send(Tool & tool)
-{
-  const double speed = std::isnan(tool.command) ? 0.0 : std::clamp(tool.command, -1.0, 1.0);
-  if (speed == 0.0) {
-    if (tool.sent != kOff) {
-      board_->set_off(tool.servo.channel);
-      tool.sent = kOff;
-    }
-    tool.speed = 0.0;
-    return;
-  }
-  const int counts = board_->counts_for(tool.servo.pulse_us(speed));
-  if (counts != tool.sent) {
-    board_->set_counts(tool.servo.channel, static_cast<uint16_t>(counts));
-    tool.sent = counts;
-  }
-  tool.speed = tool.servo.angle(counts * board_->count_us());
-}
-
 void Pca9685ServoSystem::stop()
 {
   if (!board_) {
@@ -438,10 +350,6 @@ void Pca9685ServoSystem::stop()
   for (auto & joint : joints_) {
     std::fill(joint.sent.begin(), joint.sent.end(), -1);
   }
-  for (auto & tool : tools_) {
-    tool.sent = -1;
-    tool.speed = 0.0;
-  }
 }
 
 }  // namespace tattotronix_hardware

src/tattotronix_hardware/test/test_arm_dry_run.py

@@ -78,22 +78,11 @@ def _active_controllers(self, timeout=90.0):
                 rclpy.spin_until_future_complete(self.node, future, timeout_sec=2.0)
                 if future.result() is not None:
                     active = {c.name for c in future.result().controller if c.state == 'active'}
-                    if {'joint_state_broadcaster', 'arm_controller', 'tool_controller'} <= active:
+                    if {'joint_state_broadcaster', 'arm_controller'} <= active:
                         return active
             time.sleep(0.5)
         self.fail('the controllers never became active on the PCA9685 driver')
 
-    def test_the_tool_stays_out_of_the_joint_states(self):
-        """The tool's speed is an interface, not a joint the URDF has."""
-        self._active_controllers()
-        names = []
-        self.node.create_subscription(
-            JointState, '/joint_states', lambda msg: names.append(list(msg.name)), 10)
-        deadline = time.monotonic() + 5.0
-        while not names and time.monotonic() < deadline:
-            rclpy.spin_once(self.node, timeout_sec=0.1)
-        self.assertEqual(names[-1], JOINTS)
-
     def test_the_arm_follows_a_trajectory_with_no_board_attached(self):
         self._active_controllers()
         latest = {}