Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Change interface export variant #396

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa
hardware_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override;

std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

hardware_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override;

Expand All @@ -61,10 +57,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa
double hw_start_sec_;
double hw_stop_sec_;
double hw_slowdown_;

// Store the command for the simulated robot
std::vector<double> hw_commands_;
std::vector<double> hw_states_;
};

} // namespace ros2_control_demo_example_1
Expand Down
61 changes: 18 additions & 43 deletions example_1/hardware/rrbot.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,6 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_init(
hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
hw_slowdown_ = stod(info_.hardware_parameters["example_param_hw_slowdown"]);
// END: This part here is for exemplary purposes - Please do not copy to your production code
hw_states_.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)
{
Expand Down Expand Up @@ -103,41 +101,17 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_configure
// END: This part here is for exemplary purposes - Please do not copy to your production code

// reset values always when configuring hardware
for (uint i = 0; i < hw_states_.size(); i++)
for (const auto & [name, descr] : joint_state_interfaces_)
{
hw_states_[i] = 0;
hw_commands_[i] = 0;
set_state(name, 0.0);
}

RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!");

return hardware_interface::CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface>
RRBotSystemPositionOnlyHardware::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
for (uint i = 0; i < info_.joints.size(); i++)
for (const auto & [name, descr] : joint_command_interfaces_)
{
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
}

return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
RRBotSystemPositionOnlyHardware::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
for (uint i = 0; i < info_.joints.size(); i++)
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
set_command(name, 0.0);
}
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully configured!");

return command_interfaces;
return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate(
Expand All @@ -157,9 +131,9 @@ hardware_interface::CallbackReturn RRBotSystemPositionOnlyHardware::on_activate(
// END: This part here is for exemplary purposes - Please do not copy to your production code

// command and state should be equal when starting
for (uint i = 0; i < hw_states_.size(); i++)
for (const auto & [name, descr] : joint_state_interfaces_)
{
hw_commands_[i] = hw_states_[i];
set_command(name, get_state(name));
}

RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Successfully activated!");
Expand Down Expand Up @@ -194,13 +168,14 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read(
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Reading...");

for (uint i = 0; i < hw_states_.size(); i++)
for (const auto & [name, descr] : joint_state_interfaces_)
{
auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_;
set_state(name, new_value);
// Simulate RRBot's movement
hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_;
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got state %.5f for joint %d!",
hw_states_[i], i);
RCLCPP_INFO_STREAM(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Got state " << get_state(name) << " for joint: " << name << "!");
}
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully read!");
// END: This part here is for exemplary purposes - Please do not copy to your production code
Expand All @@ -214,12 +189,12 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::write(
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Writing...");

for (uint i = 0; i < hw_commands_.size(); i++)
for (const auto & [name, descr] : joint_command_interfaces_)
{
// Simulate sending commands to the hardware
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Got command %.5f for joint %d!",
hw_commands_[i], i);
RCLCPP_INFO_STREAM(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Got command" << get_command(name) << " for joint: " << name << "!");
}
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), "Joints successfully written!");
Expand Down
Loading