Skip to content

implemented perform_command_mode_switch override in Ros2ControlSystem #846

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

Open
wants to merge 9 commits into
base: master
Choose a base branch
from
Open

implemented perform_command_mode_switch override in Ros2ControlSystem #846

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
460c1a6
Now we can switch controllers.
ralphieraccoon Sep 1, 2023
81f3c9a
Merge branch 'master' into switching-controllers
ralphieraccoon Oct 3, 2023
4b2aa16
Made Ros2ControlSystem.hpp clang-format compliant.
ralphieraccoon Oct 5, 2023
9c56d77
Included ResourceManager in Ros2ControlSystem::init()
ralphieraccoon Nov 14, 2023
98b6707
Made clang-format compliant.
ralphieraccoon Nov 14, 2023
ea27318
Ros2ControlSystem.hpp Clang-format compliant.
ralphieraccoon Nov 14, 2023
e09b6b3
Merge branch 'cyberbotics:master' into switching-controllers
ralphieraccoon Feb 6, 2024
d8f58cd
Merge branch 'master' into switching-controllers
omichel Feb 16, 2024
2fba987
Merge branch 'master' into switching-controllers
omichel Mar 2, 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: 7 additions & 1 deletion webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/macros.hpp"
Expand Down Expand Up @@ -50,7 +51,8 @@ namespace webots_ros2_control {
class Ros2ControlSystem : public Ros2ControlSystemInterface {
public:
Ros2ControlSystem();
void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) override;
void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info,
const hardware_interface::ResourceManager &resource) override;

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init(
const hardware_interface::HardwareInfo &info) override;
Expand All @@ -61,6 +63,10 @@ namespace webots_ros2_control {

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

hardware_interface::return_type perform_command_mode_switch(const std::vector<std::string> &start_interfaces,
const std::vector<std::string> &stop_interfaces) override;

hardware_interface::return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;
hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override;

Expand Down
4 changes: 3 additions & 1 deletion webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include <webots/supervisor.h>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "webots_ros2_driver/PluginInterface.hpp"
Expand All @@ -30,7 +31,8 @@
namespace webots_ros2_control {
class Ros2ControlSystemInterface : public hardware_interface::SystemInterface {
public:
virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) = 0;
virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info,
const hardware_interface::ResourceManager &resource) = 0;
};
} // namespace webots_ros2_control

Expand Down
2 changes: 1 addition & 1 deletion webots_ros2_control/src/Ros2Control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ namespace webots_ros2_control {
auto webotsSystem = std::unique_ptr<webots_ros2_control::Ros2ControlSystemInterface>(
mHardwareLoader->createUnmanagedInstance(hardwareType));
#endif
webotsSystem->init(mNode, controlHardware[i]);
webotsSystem->init(mNode, controlHardware[i], *resourceManager);
resourceManager->import_component(std::move(webotsSystem), controlHardware[i]);

// Configure and activate all components
Expand Down
58 changes: 49 additions & 9 deletions webots_ros2_control/src/Ros2ControlSystem.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,8 @@ namespace webots_ros2_control {
Ros2ControlSystem::Ros2ControlSystem() {
mNode = NULL;
}
void Ros2ControlSystem::init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) {
void Ros2ControlSystem::init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info,
const hardware_interface::ResourceManager &resource) {
mNode = node;
for (hardware_interface::ComponentInfo component : info.joints) {
Joint joint;
Expand Down Expand Up @@ -64,14 +65,16 @@ namespace webots_ros2_control {

// Configure the command interface
for (hardware_interface::InterfaceInfo commandInterface : component.command_interfaces) {
if (commandInterface.name == "position")
joint.controlPosition = true;
else if (commandInterface.name == "velocity")
joint.controlVelocity = true;
else if (commandInterface.name == "effort")
joint.controlEffort = true;
else
throw std::runtime_error("Invalid hardware info name `" + commandInterface.name + "`");
if (resource.command_interface_is_claimed(commandInterface.name)) {
if (commandInterface.name == "position")
joint.controlPosition = true;
else if (commandInterface.name == "velocity")
joint.controlVelocity = true;
else if (commandInterface.name == "effort")
joint.controlEffort = true;
else
throw std::runtime_error("Invalid hardware info name `" + commandInterface.name + "`");
}
}
if (joint.motor && joint.controlVelocity && !joint.controlPosition) {
wb_motor_set_position(joint.motor, INFINITY);
Expand Down Expand Up @@ -133,6 +136,43 @@ namespace webots_ros2_control {
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type Ros2ControlSystem::perform_command_mode_switch(
const std::vector<std::string> &start_interfaces, const std::vector<std::string> &stop_interfaces) {
for (Joint &joint : mJoints) {
for (const std::string &interface_name : stop_interfaces) {
// Clear joint control method bits corresponding to stop interfaces
if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_POSITION)) {
joint.controlPosition = false;
}
if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_VELOCITY)) {
joint.controlVelocity = false;
}
if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_EFFORT)) {
joint.controlEffort = false;
}
}

// Set joint control method bits corresponding to start interfaces
for (const std::string &interface_name : start_interfaces) {
if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_POSITION)) {
joint.controlPosition = true;
}
if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_VELOCITY)) {
joint.controlVelocity = true;
}
if (interface_name == (joint.name + "/" + hardware_interface::HW_IF_EFFORT)) {
joint.controlEffort = true;
}
}
if (joint.motor && joint.controlVelocity && !joint.controlPosition) {
wb_motor_set_position(joint.motor, INFINITY);
wb_motor_set_velocity(joint.motor, 0.0);
}
}

return hardware_interface::return_type::OK;
}

hardware_interface::return_type Ros2ControlSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
static double lastReadTime = 0;

Expand Down