Skip to content

Commit

Permalink
update demos to full variant support
Browse files Browse the repository at this point in the history
  • Loading branch information
@mamueluth
mamueluth committed Aug 13, 2024
1 parent 9e37e9d commit 1917d80
Show file tree
Hide file tree
Showing 18 changed files with 330 additions and 342 deletions.
31 changes: 24 additions & 7 deletions example_10/description/ros2_control/rrbot.ros2_control.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,28 +21,45 @@

<joint name="${prefix}joint1">
<command_interface name="position">
<param name="data_type">double</param>
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="position">
<param name="data_type">double</param>
</state_interface>
</joint>
<joint name="${prefix}joint2">
<command_interface name="position">
<param name="data_type">double</param>
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="position">
<param name="data_type">double</param>
</state_interface>
</joint>
<!-- from https://github.com/ros-controls/ros2_control/blob/f58d4072dbd1c3fd4e1f64cd10a19a2b60f59049/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp#L344 -->
<gpio name="flange_analog_IOs">
<command_interface name="analog_output1"/>
<state_interface name="analog_output1"/> <!-- Needed to know current state of the output -->
<state_interface name="analog_input1"/>
<state_interface name="analog_input2"/>
<command_interface name="analog_output1">
<param name="data_type">double</param>
</command_interface>
<state_interface name="analog_output1"> <!-- Needed to know current state of the output -->
<param name="data_type">double</param>
</state_interface>
<state_interface name="analog_input1">
<param name="data_type">double</param>
</state_interface>
<state_interface name="analog_input2">
<param name="data_type">double</param>
</state_interface>
</gpio>
<gpio name="flange_vacuum">
<command_interface name="vacuum"/>
<command_interface name="vacuum">
<param name="data_type">double</param>
</command_interface>
<state_interface name="vacuum">
<param name="data_type">double</param>
<param name="initial_value">1.0</param>
</state_interface>
</gpio>
Expand Down
30 changes: 25 additions & 5 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 @@ -68,11 +68,31 @@ 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_;
struct FlangeVacuum
{
explicit FlangeVacuum(const std::string & gpio_name) : name(gpio_name) {}
// delete move constructor since would throw because of const std::string members
// but we dont want to move this anyways so const for member is ok i guess
FlangeVacuum(FlangeVacuum && other) = delete;

const std::string name;
const std::string vacuum = name + "/vacuum";
};

struct FlangeIOs
{
explicit FlangeIOs(const std::string & gpio_name) : name(gpio_name) {}
// delete move constructor since would throw because of const std::string members
// but we dont want to move this anyways so const for member is ok i guess
FlangeIOs(FlangeIOs && other) = delete;

const std::string name;
const std::string out = name + "/analog_output1";
const std::string input_1 = name + "/analog_input1";
const std::string input_2 = name + "/analog_input2";
};
std::unique_ptr<FlangeVacuum> flange_vacuum_;
std::unique_ptr<FlangeIOs> flange_ios_;
};

} // namespace ros2_control_demo_example_10
Expand Down
121 changes: 48 additions & 73 deletions example_10/hardware/rrbot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,9 +35,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 @@ -77,6 +74,27 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init(
return hardware_interface::CallbackReturn::ERROR;
}
}
// check if we have same command and state interfaces for joint this makes iterating easier
// first check if size is equal then we only need to iterate over one of them
if (joint_state_interfaces_.size() != joint_command_interfaces_.size())
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemWithSensorHardware"),
"Expect joint CommandInterface and joint StateInterfaces to be of equal size.");
return hardware_interface::CallbackReturn::ERROR;
}
for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_)
{
if (joint_command_interfaces_.find(state_itf_name) == joint_command_interfaces_.end())
{
RCLCPP_FATAL(
rclcpp::get_logger("RRBotSystemWithSensorHardware"),
"Expect joint CommandInterface and joint StateInterfaces to be equal but StateInterface "
"includes<%s> which is not included in CommandInterfaces.",
state_itf_name.c_str());
return hardware_interface::CallbackReturn::ERROR;
}
}

// RRBotSystemWithGPIOHardware has exactly two GPIO components
if (info_.gpios.size() != 2)
Expand Down Expand Up @@ -116,6 +134,9 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init(
info_.gpios[0].state_interfaces.size(), 1);
return hardware_interface::CallbackReturn::ERROR;
}
// set gpios
flange_ios_ = std::make_unique<FlangeIOs>(info_.gpios[0].name);
flange_vacuum_ = std::make_unique<FlangeVacuum>(info_.gpios[1].name);

return hardware_interface::CallbackReturn::SUCCESS;
}
Expand All @@ -128,69 +149,23 @@ 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(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "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++)
{
state_interfaces.emplace_back(hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
}

RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "State interfaces:");
hw_gpio_in_.resize(4);
size_t ct = 0;
for (size_t i = 0; i < info_.gpios.size(); i++)
for (const auto & [itf_name, itf_descr] : joint_state_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(
rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s",
info_.gpios.at(i).name.c_str(), state_if.name.c_str());
}
set_state(itf_name, 0.0);
set_command(itf_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 & [gpio_state, gpio_state_desc] : gpio_state_interfaces_)
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
set_state(gpio_state, 0.0);
}
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Command interfaces:");
hw_gpio_out_.resize(2);
size_t ct = 0;
for (size_t i = 0; i < info_.gpios.size(); i++)
for (const auto & [gpio_cmd, gpio_cmd_desc] : 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(
rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s",
info_.gpios.at(i).name.c_str(), command_if.name.c_str());
}
set_command(gpio_cmd, 0.0);
}

return command_interfaces;
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully configured!");

return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate(
Expand All @@ -201,9 +176,9 @@ 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 & [itf_name, itf_descr] : joint_state_interfaces_)
{
hw_commands_[i] = hw_states_[i];
set_command(itf_name, get_state(itf_name));
}

RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully activated!");
Expand All @@ -227,26 +202,26 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::read(
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Reading...");

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

// mirror GPIOs back
hw_gpio_in_[0] = hw_gpio_out_[0];
hw_gpio_in_[3] = hw_gpio_out_[1];
set_state(flange_ios_->out, get_command(flange_ios_->out));
set_state(flange_vacuum_->vacuum, get_command(flange_vacuum_->vacuum));
// random inputs
unsigned int seed = time(NULL) + 1;
hw_gpio_in_[1] = static_cast<float>(rand_r(&seed));
set_state(flange_ios_->input_1, static_cast<float>(rand_r(&seed)));
seed = time(NULL) + 2;
hw_gpio_in_[2] = static_cast<float>(rand_r(&seed));
set_state(flange_ios_->input_2, static_cast<float>(rand_r(&seed)));

for (uint i = 0; i < hw_gpio_in_.size(); i++)
for (const auto & [gpio_state, gpio_state_desc] : gpio_state_interfaces_)
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Read %.1f from GP input %d!",
hw_gpio_in_[i], i);
rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Read %.1f from GP input %s!",
get_state(gpio_state), gpio_state.c_str());
}
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully read!");
// END: This part here is for exemplary purposes - Please do not copy to your production code
Expand All @@ -260,11 +235,11 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::write(
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Writing...");

for (uint i = 0; i < hw_gpio_out_.size(); i++)
for (const auto & [gpio_cmd, gpio_cmd_desc] : gpio_command_interfaces_)
{
RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Got command %.1f for GP output %d!",
hw_gpio_out_[i], i);
rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Got command %.1f for GP output %s!",
get_command(gpio_cmd), gpio_cmd.c_str());
}
RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully written!");
// END: This part here is for exemplary purposes - Please do not copy to your production code
Expand Down
10 changes: 8 additions & 2 deletions ...rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,9 @@
<param name="example_param_socket_port">23286</param>
</hardware>
<joint name="joint1">
<state_interface name="position"/>
<state_interface name="position">
<param name="data_type">double</param>
</state_interface>
</joint>
</ros2_control>
<ros2_control name="RRBotModularPositionSensorJoint2" type="sensor">
Expand All @@ -29,7 +31,9 @@
<param name="example_param_socket_port">23287</param>
</hardware>
<joint name="joint2">
<state_interface name="position"/>
<state_interface name="position">
<param name="data_type">double</param>
</state_interface>
</joint>
</ros2_control>

Expand All @@ -42,6 +46,7 @@
</hardware>
<joint name="joint1">
<command_interface name="velocity">
<param name="data_type">double</param>
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
Expand All @@ -56,6 +61,7 @@
</hardware>
<joint name="joint2">
<command_interface name="velocity">
<param name="data_type">double</param>
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
Expand Down
4 changes: 1 addition & 3 deletions example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,9 +74,7 @@ class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterfac
// Parameters for the RRBot simulation
double hw_start_sec_;
double hw_stop_sec_;

// Store the command for the simulated robot
double hw_joint_command_;
std::string vel_itf_;

// Fake "mechanical connection" between actuator and sensor using sockets
struct sockaddr_in address_;
Expand Down
2 changes: 1 addition & 1 deletion ...e_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ class RRBotSensorPositionFeedback : public hardware_interface::SensorInterface
// Store the command for the simulated robot
double measured_velocity; // Local variable, but avoid initialization on each read
double last_measured_velocity_;
double hw_joint_state_;
std::string pos_itf_;

// Timestamps to calculate position for velocity
rclcpp::Clock clock_;
Expand Down
Loading

0 comments on commit 1917d80

Please sign in to comment.