thomas/Cobots
2
1
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Added comments

Browse source
This commit is contained in:
Thomas Muller 2022-03-15 21:17:08 -04:00
parent 2d858ab961
commit 9fdd5cf353
Signed by: thomas
GPG key ID: AF006EB730564952
1 changed files with 25 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

26
src/ar3_hardware/src/ar3_system.cpp
View file

@ -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);