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.

Sign up for GitHub

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

Jump to bottom

Change interface export variant #396

Merged
merged 6 commits into from
Oct 8, 2024
Merged

Change interface export variant #396

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
8ad2000
adjust example 1 with new export of interfaces
mamueluth Dec 7, 2023
1f0b400
adapt to changes
mamueluth Dec 8, 2023
95e2b19
adjust to descriptions
mamueluth Dec 12, 2023
c516af7
adapt example 1 to stored handle-names/descriptions in map
mamueluth Dec 19, 2023
e1ebe6b
adjust to usage of get_/set_state or command functions
mamueluth Dec 20, 2023
5f5a2e7
Merge branch 'master' into change_interface_export_variant
destogl Oct 8, 2024
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
8 changes: 0 additions & 8 deletions example_1/hardware/include/ros2_control_demo_example_1/rrbot.hpp
View file
Open in desktop
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
View file
Open in desktop
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