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

Change interface export step 1 #9

Draft
wants to merge 5 commits into
base: master
Choose a base branch
from
Draft

Change interface export step 1 #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
a8ca44b
adjust example 1 with new export of interfaces
mamueluth Dec 7, 2023
e81d85f
adapt to changes
mamueluth Dec 8, 2023
e66b7a8
adjust to descriptions
mamueluth Dec 12, 2023
714f0f0
adapt example 1 to stored handle-names/descriptions in map
mamueluth Dec 19, 2023
abfc300
adjust to usage of get_/set_state or command functions
mamueluth Dec 20, 2023
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
10 changes: 0 additions & 10 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 @@ -43,12 +43,6 @@ class RRBotSystemPositionOnlyHardware : public hardware_interface::SystemInterfa
hardware_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override;

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

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

ROS2_CONTROL_DEMO_EXAMPLE_1_PUBLIC
hardware_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override;
Expand All @@ -70,10 +64,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