Added comments

src/ar3_hardware/src/ar3_system.cpp

@@ -14,6 +14,7 @@ namespace ar3_hardware
 hardware_interface::return_type Ar3SystemHardware::configure(
   const hardware_interface::HardwareInfo & info)
 {
+  // Initialize default configuration
   if (configure_default(info) != hardware_interface::return_type::OK)
   {
     return hardware_interface::return_type::ERROR;
@@ -21,12 +22,15 @@ hardware_interface::return_type Ar3SystemHardware::configure(
 
   //hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
   //hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
+
+  // Resize position and command vectors to match actual configuration
+  // This makes sure a default value is initialized for each joint and saves
   hw_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
   hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
 
   for (const hardware_interface::ComponentInfo & joint : info_.joints)
   {
-    // DiffBotSystem has exactly two states and one command interface on each joint
+    // We only use one command interface
     if (joint.command_interfaces.size() != 1)
     {
       RCLCPP_FATAL(
@@ -36,6 +40,7 @@ hardware_interface::return_type Ar3SystemHardware::configure(
       return hardware_interface::return_type::ERROR;
     }
 
+    // Make sure the command interface is in position mode
     if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
     {
       RCLCPP_FATAL(
@@ -45,6 +50,7 @@ hardware_interface::return_type Ar3SystemHardware::configure(
       return hardware_interface::return_type::ERROR;
     }
 
+    // We only publish to one state interface
     if (joint.state_interfaces.size() != 1)
     {
       RCLCPP_FATAL(
@@ -54,6 +60,7 @@ hardware_interface::return_type Ar3SystemHardware::configure(
       return hardware_interface::return_type::ERROR;
     }
 
+    // Make sure the state interface is in position mode
     if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
     {
       RCLCPP_FATAL(
@@ -71,6 +78,7 @@ hardware_interface::return_type Ar3SystemHardware::configure(
 
 std::vector<hardware_interface::StateInterface> Ar3SystemHardware::export_state_interfaces()
 {
+  // Tell the controller about all joints that we publish state info for
   std::vector<hardware_interface::StateInterface> state_interfaces;
   for (auto i = 0u; i < info_.joints.size(); i++)
   {
@@ -83,6 +91,7 @@ std::vector<hardware_interface::StateInterface> Ar3SystemHardware::export_state_
 
 std::vector<hardware_interface::CommandInterface> Ar3SystemHardware::export_command_interfaces()
 {
+  // Tell the controller about all joints that we take commands from
   std::vector<hardware_interface::CommandInterface> command_interfaces;
   for (auto i = 0u; i < info_.joints.size(); i++)
   {
@@ -107,6 +116,12 @@ hardware_interface::return_type Ar3SystemHardware::start()
     }
   }
 
+  // Hack to make sure the node stays running by putting it in its own thread.
+  // this is because we wrote the hardware interface in python earlier and
+  // the driver just listens on some subscribers. This allows a node in this
+  // controller to publish values to that driver
+  // The correct thing to do would be to move all of the driver code into this
+  // controller and remove the node, publishers, and subscribers entierly
   executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
   nh_ = std::make_shared<rclcpp::Node>("ar3_hardware");
   executor_->add_node(nh_);
@@ -118,6 +133,8 @@ hardware_interface::return_type Ar3SystemHardware::start()
   };
   thread_executor_spin_ = std::thread(spin);
 
+  // Create joint state publishers
+  // TODO: Don't use Float32, use Pose
   j1_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j1", 10);
   j2_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j2", 10);
   j3_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j3", 10);
@@ -125,6 +142,8 @@ hardware_interface::return_type Ar3SystemHardware::start()
   j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
   j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j6", 10);
 
+  // Create joint command subscribers
+  // TODO: Don't use Float32, use Pose
   s_j1 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j1", 10, std::bind(&Ar3SystemHardware::update_j1_cb, this, std::placeholders::_1));
   s_j2 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j2", 10, std::bind(&Ar3SystemHardware::update_j2_cb, this, std::placeholders::_1));
   s_j3 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j3", 10, std::bind(&Ar3SystemHardware::update_j3_cb, this, std::placeholders::_1));
@@ -150,6 +169,10 @@ hardware_interface::return_type Ar3SystemHardware::stop()
   return hardware_interface::return_type::OK;
 }
 
+// These callbacks are called from the command subscribers, they update
+// the hw_positions_ array with the current joint state that will be
+// sent to the controller.
+// Again, this is wrong and these values should be filled in by the read() function
 void Ar3SystemHardware::update_j1_cb(std_msgs::msg::Float32::SharedPtr msg) {
     hw_positions_[0] = msg->data;
 }
@@ -201,6 +224,7 @@ hardware_interface::return_type ar3_hardware::Ar3SystemHardware::write()
 {
   //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Writing...");
 
+  // Publish joint commands to the arm driver
   auto message = std_msgs::msg::Float32();
   message.data = hw_commands_[0];
   j1_pub_->publish(message);