From 9fdd5cf3532be0220bedaff3540fdc7ec76fe0ae Mon Sep 17 00:00:00 2001 From: Quantum Date: Tue, 15 Mar 2022 21:17:08 -0400 Subject: [PATCH] Added comments --- src/ar3_hardware/src/ar3_system.cpp | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/src/ar3_hardware/src/ar3_system.cpp b/src/ar3_hardware/src/ar3_system.cpp index 0c831a6..8da7259 100644 --- a/src/ar3_hardware/src/ar3_system.cpp +++ b/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::quiet_NaN()); hw_commands_.resize(info_.joints.size(), std::numeric_limits::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 Ar3SystemHardware::export_state_interfaces() { + // Tell the controller about all joints that we publish state info for std::vector state_interfaces; for (auto i = 0u; i < info_.joints.size(); i++) { @@ -83,6 +91,7 @@ std::vector Ar3SystemHardware::export_state_ std::vector Ar3SystemHardware::export_command_interfaces() { + // Tell the controller about all joints that we take commands from std::vector 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(); nh_ = std::make_shared("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("/move/j1", 10); j2_pub_ = nh_->create_publisher("/move/j2", 10); j3_pub_ = nh_->create_publisher("/move/j3", 10); @@ -125,6 +142,8 @@ hardware_interface::return_type Ar3SystemHardware::start() j5_pub_ = nh_->create_publisher("/move/j5", 10); j6_pub_ = nh_->create_publisher("/move/j6", 10); + // Create joint command subscribers + // TODO: Don't use Float32, use Pose s_j1 = nh_->create_subscription("/update/j1", 10, std::bind(&Ar3SystemHardware::update_j1_cb, this, std::placeholders::_1)); s_j2 = nh_->create_subscription("/update/j2", 10, std::bind(&Ar3SystemHardware::update_j2_cb, this, std::placeholders::_1)); s_j3 = nh_->create_subscription("/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);