Exlusive hw interface rolling

controller_manager/CMakeLists.txt

@@ -171,6 +171,20 @@ if(BUILD_TESTING)
  DESTINATION lib
  )
 
+ add_library(test_controller_failed_activate SHARED
+ test/test_controller_failed_activate/test_controller_failed_activate.cpp
+ )
+ target_link_libraries(test_controller_failed_activate PUBLIC
+ controller_manager
+ )
+ target_compile_definitions(test_controller_failed_activate PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
+ pluginlib_export_plugin_description_file(
+ controller_interface test/test_controller_failed_activate/test_controller_failed_activate.xml)
+ install(
+ TARGETS test_controller_failed_activate
+ DESTINATION lib
+ )
+
  ament_add_gmock(test_release_interfaces
  test/test_release_interfaces.cpp
  APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>

controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp

@@ -0,0 +1,66 @@
+// Copyright 2021 Department of Engineering Cybernetics, NTNU.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "test_controller_failed_activate.hpp"
+
+#include <memory>
+#include <string>
+
+#include "lifecycle_msgs/msg/transition.hpp"
+
+namespace test_controller_failed_activate
+{
+TestControllerFailedActivate::TestControllerFailedActivate()
+: controller_interface::ControllerInterface()
+{
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerFailedActivate::on_init()
+{
+ return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type TestControllerFailedActivate::update(
+ const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+{
+ return controller_interface::return_type::OK;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerFailedActivate::on_configure(const rclcpp_lifecycle::State & /*previous_state&*/)
+{
+ return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerFailedActivate::on_activate(const rclcpp_lifecycle::State & /*previous_state&*/)
+{
+ // Simply simulate a controller that can not be activated
+ return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestControllerFailedActivate::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/)
+{
+ return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+} // namespace test_controller_failed_activate
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+ test_controller_failed_activate::TestControllerFailedActivate,
+ controller_interface::ControllerInterface)

controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp

@@ -0,0 +1,75 @@
+// Copyright 2020 Department of Engineering Cybernetics, NTNU
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_
+#define TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_
+
+#include <memory>
+#include <string>
+
+#include "controller_interface/visibility_control.h"
+#include "controller_manager/controller_manager.hpp"
+
+namespace test_controller_failed_activate
+{
+// Corresponds to the name listed within the pluginglib xml
+constexpr char TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME[] =
+ "controller_manager/test_controller_failed_activate";
+// Corresponds to the command interface to claim
+constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint2/velocity";
+class TestControllerFailedActivate : public controller_interface::ControllerInterface
+{
+public:
+ CONTROLLER_MANAGER_PUBLIC
+ TestControllerFailedActivate();
+
+ CONTROLLER_MANAGER_PUBLIC
+ virtual ~TestControllerFailedActivate() = default;
+
+ controller_interface::InterfaceConfiguration command_interface_configuration() const override
+ {
+ return controller_interface::InterfaceConfiguration{
+ controller_interface::interface_configuration_type::INDIVIDUAL,
+ {TEST_CONTROLLER_COMMAND_INTERFACE}};
+ }
+
+ controller_interface::InterfaceConfiguration state_interface_configuration() const override
+ {
+ return controller_interface::InterfaceConfiguration{
+ controller_interface::interface_configuration_type::NONE};
+ }
+
+ CONTROLLER_MANAGER_PUBLIC
+ controller_interface::return_type update(
+ const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+ CONTROLLER_MANAGER_PUBLIC
+ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override;
+
+ CONTROLLER_MANAGER_PUBLIC
+ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
+ const rclcpp_lifecycle::State & previous_state) override;
+
+ CONTROLLER_MANAGER_PUBLIC
+ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(
+ const rclcpp_lifecycle::State & previous_state) override;
+
+ CONTROLLER_MANAGER_PUBLIC
+ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(
+ const rclcpp_lifecycle::State & previous_state) override;
+};
+
+} // namespace test_controller_failed_activate
+
+#endif // TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_

controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml

@@ -0,0 +1,9 @@
+<library path="test_controller_failed_activate">
+
+ <class name="controller_manager/test_controller_failed_activate" type="test_controller_failed_activate::TestControllerFailedActivate" base_class_type="controller_interface::ControllerInterface">
+ <description>
+ Controller used for testing
+ </description>
+ </class>
+
+</library>

hardware_interface_testing/CMakeLists.txt

@@ -21,12 +21,14 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
 endforeach()
 
 add_library(test_components SHARED
-test/test_components/test_actuator.cpp
-test/test_components/test_sensor.cpp
-test/test_components/test_system.cpp)
+ test/test_components/test_actuator.cpp
+ test/test_components/test_sensor.cpp
+ test/test_components/test_system.cpp
+ test/test_components/test_actuator_exclusive_interfaces.cpp
+)
 ament_target_dependencies(test_components hardware_interface pluginlib ros2_control_test_assets)
 install(TARGETS test_components
-DESTINATION lib
+ DESTINATION lib
 )
 pluginlib_export_plugin_description_file(
 hardware_interface test/test_components/test_components.xml)

hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp

@@ -0,0 +1,173 @@
+// Copyright 2020 ros2_control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <vector>
+
+#include "hardware_interface/actuator_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
+
+using hardware_interface::ActuatorInterface;
+using hardware_interface::CommandInterface;
+using hardware_interface::return_type;
+using hardware_interface::StateInterface;
+
+static std::pair<std::string, std::string> extract_joint_and_interface(
+ const std::string & full_name)
+{
+ // Signature is: interface/joint
+ const auto joint_name = full_name.substr(0, full_name.find_last_of('/'));
+ const auto interface_name = full_name.substr(full_name.find_last_of('/') + 1);
+
+ return {joint_name, interface_name};
+}
+struct JointState
+{
+ double pos;
+ double vel;
+ double effort;
+};
+
+class TestActuatorExclusiveInterfaces : public ActuatorInterface
+{
+ CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
+ {
+ if (ActuatorInterface::on_init(info) != CallbackReturn::SUCCESS)
+ {
+ return CallbackReturn::ERROR;
+ }
+
+ for (const auto & j : info.joints)
+ {
+ (void)j; // Suppress unused warning
+ current_states_.emplace_back(JointState{});
+ }
+
+ return CallbackReturn::SUCCESS;
+ }
+
+ std::vector<StateInterface> export_state_interfaces() override
+ {
+ std::vector<StateInterface> state_interfaces;
+ for (std::size_t i = 0; i < info_.joints.size(); ++i)
+ {
+ const auto & joint = info_.joints[i];
+
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ joint.name, hardware_interface::HW_IF_POSITION, &current_states_.at(i).pos));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ joint.name, hardware_interface::HW_IF_VELOCITY, &current_states_.at(i).vel));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ joint.name, hardware_interface::HW_IF_EFFORT, &current_states_.at(i).effort));
+ }
+ return state_interfaces;
+ }
+
+ std::vector<CommandInterface> export_command_interfaces() override
+ {
+ std::vector<CommandInterface> command_interfaces;
+ for (std::size_t i = 0; i < info_.joints.size(); ++i)
+ {
+ const auto & joint = info_.joints[i];
+
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ joint.name, hardware_interface::HW_IF_POSITION, &current_states_.at(i).pos));
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ joint.name, hardware_interface::HW_IF_VELOCITY, &current_states_.at(i).vel));
+ command_interfaces.emplace_back(hardware_interface::CommandInterface(
+ joint.name, hardware_interface::HW_IF_EFFORT, &current_states_.at(i).effort));
+ }
+ return command_interfaces;
+ }
+
+ hardware_interface::return_type prepare_command_mode_switch(
+ const std::vector<std::string> & start_interfaces,
+ const std::vector<std::string> & stop_interfaces) override
+ {
+ std::vector<std::string> claimed_joint_copy = currently_claimed_joints_;
+
+ for (const auto & interface : stop_interfaces)
+ {
+ const auto && [joint_name, interface_name] = extract_joint_and_interface(interface);
+ if (
+ interface_name == hardware_interface::HW_IF_POSITION ||
+ interface_name == hardware_interface::HW_IF_VELOCITY ||
+ interface_name == hardware_interface::HW_IF_EFFORT)
+ {
+ claimed_joint_copy.erase(
+ std::remove(claimed_joint_copy.begin(), claimed_joint_copy.end(), joint_name),
+ claimed_joint_copy.end());
+ }
+ }
+
+ for (const auto & interface : start_interfaces)
+ {
+ const auto && [joint_name, interface_name] = extract_joint_and_interface(interface);
+ if (
+ interface_name == hardware_interface::HW_IF_POSITION ||
+ interface_name == hardware_interface::HW_IF_VELOCITY ||
+ interface_name == hardware_interface::HW_IF_EFFORT)
+ {
+ if (
+ std::find(claimed_joint_copy.begin(), claimed_joint_copy.end(), joint_name) !=
+ claimed_joint_copy.end())
+ {
+ return hardware_interface::return_type::ERROR;
+ }
+ }
+ }
+ return hardware_interface::return_type::OK;
+ }
+
+ hardware_interface::return_type perform_command_mode_switch(
+ const std::vector<std::string> & start_interfaces,
+ const std::vector<std::string> & stop_interfaces) override
+ {
+ for (const auto & interface : stop_interfaces)
+ {
+ const auto && [joint_name, interface_name] = extract_joint_and_interface(interface);
+ currently_claimed_joints_.erase(
+ std::remove(currently_claimed_joints_.begin(), currently_claimed_joints_.end(), joint_name),
+ currently_claimed_joints_.end());
+ }
+
+ for (const auto & interface : start_interfaces)
+ {
+ const auto && [joint_name, interface_name] = extract_joint_and_interface(interface);
+
+ // TODO make sure it is not possible to claim multiple interfaces of the same axis
+ currently_claimed_joints_.push_back(joint_name);
+ }
+
+ return hardware_interface::return_type::OK;
+ }
+
+ return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
+ {
+ return return_type::OK;
+ }
+
+ return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
+ {
+ return return_type::OK;
+ }
+
+private:
+ std::vector<std::string> currently_claimed_joints_;
+ std::vector<JointState> current_states_;
+};
+
+#include "pluginlib/class_list_macros.hpp" // NOLINT
+PLUGINLIB_EXPORT_CLASS(TestActuatorExclusiveInterfaces, hardware_interface::ActuatorInterface)

hardware_interface_testing/test/test_components/test_components.xml

@@ -5,6 +5,12 @@
  Test Actuator
  </description>
  </class>
+ <class name="test_actuator_exclusive_interfaces" type="TestActuatorExclusiveInterfaces" base_class_type="hardware_interface::ActuatorInterface">
+ <description>
+ Test Actuator
+ </description>
+ </class>
+
 
  <class name="test_sensor" type="TestSensor" base_class_type="hardware_interface::SensorInterface">
  <description>