Skip to content

Commit

Permalink
Change interface export variant of all examples (#623)
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich authored Dec 4, 2024
1 parent 34629b5 commit 75a6e0c
Show file tree
Hide file tree
Showing 26 changed files with 365 additions and 730 deletions.
13 changes: 3 additions & 10 deletions example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,6 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface
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 @@ -58,12 +54,9 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface

private:
// Parameters for the RRBot simulation

// Store the command and state interfaces for the simulated robot
std::vector<double> hw_commands_;
std::vector<double> hw_states_;
std::vector<double> hw_gpio_in_;
std::vector<double> hw_gpio_out_;
double hw_start_sec_;
double hw_stop_sec_;
double hw_slowdown_;
};

} // namespace ros2_control_demo_example_10
Expand Down
106 changes: 37 additions & 69 deletions example_10/hardware/rrbot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,9 +37,6 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init(
return hardware_interface::CallbackReturn::ERROR;
}

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)
{
// RRBotSystemPositionOnly has exactly one state and command interface on each joint
Expand Down Expand Up @@ -123,67 +120,25 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_configure(
// END: This part here is for exemplary purposes - Please do not copy to your production code

// reset values always when configuring hardware
std::fill(hw_states_.begin(), hw_states_.end(), 0);
std::fill(hw_commands_.begin(), hw_commands_.end(), 0);
std::fill(hw_gpio_in_.begin(), hw_gpio_in_.end(), 0);
std::fill(hw_gpio_out_.begin(), hw_gpio_out_.end(), 0);

RCLCPP_INFO(get_logger(), "Successfully configured!");

return hardware_interface::CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface>
RRBotSystemWithGPIOHardware::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_state_interfaces_)
{
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
set_state(name, 0.0);
}

RCLCPP_INFO(get_logger(), "State interfaces:");
hw_gpio_in_.resize(4);
size_t ct = 0;
for (size_t i = 0; i < info_.gpios.size(); i++)
for (const auto & [name, descr] : joint_command_interfaces_)
{
for (auto state_if : info_.gpios.at(i).state_interfaces)
{
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.gpios.at(i).name, state_if.name, &hw_gpio_in_[ct++]));
RCLCPP_INFO(
get_logger(), "Added %s/%s", info_.gpios.at(i).name.c_str(), state_if.name.c_str());
}
set_command(name, 0.0);
}

return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
RRBotSystemWithGPIOHardware::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
for (uint i = 0; i < info_.joints.size(); i++)
for (const auto & [name, descr] : gpio_state_interfaces_)
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
set_state(name, 0.0);
}
RCLCPP_INFO(get_logger(), "Command interfaces:");
hw_gpio_out_.resize(2);
size_t ct = 0;
for (size_t i = 0; i < info_.gpios.size(); i++)
for (const auto & [name, descr] : gpio_command_interfaces_)
{
for (auto command_if : info_.gpios.at(i).command_interfaces)
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.gpios.at(i).name, command_if.name, &hw_gpio_out_[ct++]));
RCLCPP_INFO(
get_logger(), "Added %s/%s", info_.gpios.at(i).name.c_str(), command_if.name.c_str());
}
set_command(name, 0.0);
}
RCLCPP_INFO(get_logger(), "Successfully configured!");

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

hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate(
Expand All @@ -194,9 +149,13 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::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_)
{
set_command(name, get_state(name));
}
for (const auto & [name, descr] : gpio_command_interfaces_)
{
hw_commands_[i] = hw_states_[i];
set_command(name, get_state(name));
}

RCLCPP_INFO(get_logger(), "Successfully activated!");
Expand All @@ -221,25 +180,33 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::read(
std::stringstream ss;
ss << "Reading states:";

for (uint i = 0; i < hw_states_.size(); i++)
for (const auto & [name, descr] : joint_state_interfaces_)
{
// Simulate RRBot's movement
hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]);
auto new_value = get_state(name) + (get_command(name) - get_state(name)) / hw_slowdown_;
set_state(name, new_value);
}

for (const auto & [name, descr] : gpio_command_interfaces_)
{
// mirror GPIOs back
set_state(name, get_command(name));
}

// mirror GPIOs back
hw_gpio_in_[0] = hw_gpio_out_[0];
hw_gpio_in_[3] = hw_gpio_out_[1];
// random inputs
// random inputs analog_input1 and analog_input2
unsigned int seed = time(NULL) + 1;
hw_gpio_in_[1] = static_cast<float>(rand_r(&seed));
set_state(
info_.gpios[0].name + "/" + info_.gpios[0].state_interfaces[1].name,
static_cast<float>(rand_r(&seed)));
seed = time(NULL) + 2;
hw_gpio_in_[2] = static_cast<float>(rand_r(&seed));
set_state(
info_.gpios[0].name + "/" + info_.gpios[0].state_interfaces[2].name,
static_cast<float>(rand_r(&seed)));

for (uint i = 0; i < hw_gpio_in_.size(); i++)
for (const auto & [name, descr] : gpio_state_interfaces_)
{
ss << std::fixed << std::setprecision(2) << std::endl
<< "\t" << hw_gpio_in_[i] << " from GPIO input '" << static_cast<int>(i) << "'";
<< "\t" << get_state(name) << " from GPIO input '" << name << "'";
}
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str());
// END: This part here is for exemplary purposes - Please do not copy to your production code
Expand All @@ -254,10 +221,11 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::write(
std::stringstream ss;
ss << "Writing commands:";

for (uint i = 0; i < hw_gpio_out_.size(); i++)
for (const auto & [name, descr] : gpio_command_interfaces_)
{
// Simulate sending commands to the hardware
ss << std::fixed << std::setprecision(2) << std::endl
<< "\t" << hw_gpio_out_[i] << " for GPIO output '" << static_cast<int>(i) << "'";
<< "\t" << get_command(name) << " for GPIO output '" << name << "'";
}
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str());
// END: This part here is for exemplary purposes - Please do not copy to your production code
Expand Down
118 changes: 40 additions & 78 deletions example_11/hardware/carlikebot_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
// Steering joints have a position command interface and a position state interface
if (joint_is_steering)
{
steering_joint_ = joint.name;
RCLCPP_INFO(get_logger(), "Joint '%s' is a steering joint.", joint.name.c_str());

if (joint.command_interfaces.size() != 1)
Expand Down Expand Up @@ -93,6 +94,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
else
{
RCLCPP_INFO(get_logger(), "Joint '%s' is a drive joint.", joint.name.c_str());
traction_joint_ = joint.name;

// Drive joints have a velocity command interface and a velocity state interface
if (joint.command_interfaces.size() != 1)
Expand Down Expand Up @@ -143,69 +145,33 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
// // END: This part here is for exemplary purposes - Please do not copy to your production code

hw_interfaces_["steering"] = Joint("virtual_front_wheel_joint");

hw_interfaces_["traction"] = Joint("virtual_rear_wheel_joint");

return hardware_interface::CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> CarlikeBotSystemHardware::export_state_interfaces()
hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/)
{
std::vector<hardware_interface::StateInterface> state_interfaces;
RCLCPP_INFO(get_logger(), "Configuring ...please wait...");

for (auto & joint : hw_interfaces_)
for (auto i = 0; i < hw_start_sec_; i++)
{
state_interfaces.emplace_back(hardware_interface::StateInterface(
joint.second.joint_name, hardware_interface::HW_IF_POSITION, &joint.second.state.position));

if (joint.first == "traction")
{
state_interfaces.emplace_back(hardware_interface::StateInterface(
joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, &joint.second.state.velocity));
}
rclcpp::sleep_for(std::chrono::seconds(1));
RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i);
}

RCLCPP_INFO(get_logger(), "Exported %zu state interfaces.", state_interfaces.size());

for (auto s : state_interfaces)
// reset values always when configuring hardware
for (const auto & [name, descr] : joint_state_interfaces_)
{
RCLCPP_INFO(get_logger(), "Exported state interface '%s'.", s.get_name().c_str());
set_state(name, 0.0);
}

return state_interfaces;
}

std::vector<hardware_interface::CommandInterface>
CarlikeBotSystemHardware::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;

for (auto & joint : hw_interfaces_)
for (const auto & [name, descr] : joint_command_interfaces_)
{
if (joint.first == "steering")
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(
joint.second.joint_name, hardware_interface::HW_IF_POSITION,
&joint.second.command.position));
}
else if (joint.first == "traction")
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(
joint.second.joint_name, hardware_interface::HW_IF_VELOCITY,
&joint.second.command.velocity));
}
set_command(name, 0.0);
}

RCLCPP_INFO(get_logger(), "Exported %zu command interfaces.", command_interfaces.size());

for (auto i = 0u; i < command_interfaces.size(); i++)
{
RCLCPP_INFO(
get_logger(), "Exported command interface '%s'.", command_interfaces[i].get_name().c_str());
}
RCLCPP_INFO(get_logger(), "Successfully configured!");

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

hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate(
Expand All @@ -219,20 +185,10 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate(
RCLCPP_INFO(get_logger(), "%.1f seconds left...", hw_start_sec_ - i);
}

for (auto & joint : hw_interfaces_)
// command and state should be equal when starting
for (const auto & [name, descr] : joint_command_interfaces_)
{
joint.second.state.position = 0.0;

if (joint.first == "traction")
{
joint.second.state.velocity = 0.0;
joint.second.command.velocity = 0.0;
}

else if (joint.first == "steering")
{
joint.second.command.position = 0.0;
}
set_command(name, get_state(name));
}

RCLCPP_INFO(get_logger(), "Successfully activated!");
Expand Down Expand Up @@ -261,26 +217,32 @@ hardware_interface::return_type CarlikeBotSystemHardware::read(
const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code

hw_interfaces_["steering"].state.position = hw_interfaces_["steering"].command.position;

hw_interfaces_["traction"].state.velocity = hw_interfaces_["traction"].command.velocity;
hw_interfaces_["traction"].state.position +=
hw_interfaces_["traction"].state.velocity * period.seconds();
// update states from commands and integrate velocity to position
set_state(
steering_joint_ + "/" + hardware_interface::HW_IF_POSITION,
get_command(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION));

set_state(
traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY,
get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY));
set_state(
traction_joint_ + "/" + hardware_interface::HW_IF_POSITION,
get_state(traction_joint_ + "/" + hardware_interface::HW_IF_POSITION) +
get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY) * period.seconds());

std::stringstream ss;
ss << "Reading states:";

ss << std::fixed << std::setprecision(2) << std::endl
<< "\t"
<< "position: " << hw_interfaces_["steering"].state.position << " for joint '"
<< hw_interfaces_["steering"].joint_name.c_str() << "'" << std::endl
<< "position: " << get_state(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION)
<< " for joint '" << steering_joint_ << "'" << std::endl
<< "\t"
<< "position: " << hw_interfaces_["traction"].state.position << " for joint '"
<< hw_interfaces_["traction"].joint_name.c_str() << "'" << std::endl
<< "position: " << get_state(traction_joint_ + "/" + hardware_interface::HW_IF_POSITION)
<< " for joint '" << traction_joint_ << "'" << std::endl
<< "\t"
<< "velocity: " << hw_interfaces_["traction"].state.velocity << " for joint '"
<< hw_interfaces_["traction"].joint_name.c_str() << "'";
<< "velocity: " << get_state(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY)
<< " for joint '" << traction_joint_ << "'";

RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str());

Expand All @@ -298,11 +260,11 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH

ss << std::fixed << std::setprecision(2) << std::endl
<< "\t"
<< "position: " << hw_interfaces_["steering"].command.position << " for joint '"
<< hw_interfaces_["steering"].joint_name.c_str() << "'" << std::endl
<< "position: " << get_command(steering_joint_ + "/" + hardware_interface::HW_IF_POSITION)
<< " for joint '" << steering_joint_ << "'" << std::endl
<< "\t"
<< "velocity: " << hw_interfaces_["traction"].command.velocity << " for joint '"
<< hw_interfaces_["traction"].joint_name.c_str() << "'";
<< "velocity: " << get_command(traction_joint_ + "/" + hardware_interface::HW_IF_VELOCITY)
<< " for joint '" << traction_joint_ << "'";

RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str());
// END: This part here is for exemplary purposes - Please do not copy to your production code
Expand Down
11 changes: 5 additions & 6 deletions example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,9 +63,8 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface
hardware_interface::CallbackReturn on_init(
const hardware_interface::HardwareInfo & info) override;

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

std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
hardware_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override;

hardware_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override;
Expand All @@ -84,9 +83,9 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface
double hw_start_sec_;
double hw_stop_sec_;

// std::vector<std::tuple<std::string, double, double>>
// hw_interfaces_; // name of joint, state, command
std::map<std::string, Joint> hw_interfaces_;
// joint names
std::string steering_joint_;
std::string traction_joint_;
};

} // namespace ros2_control_demo_example_11
Expand Down
Loading

0 comments on commit 75a6e0c

Please sign in to comment.