diff --git a/hardware_interface/include/fake_components/generic_system.hpp b/hardware_interface/include/fake_components/generic_system.hpp index 1540ffd6fd..3dfb2b9b70 100644 --- a/hardware_interface/include/fake_components/generic_system.hpp +++ b/hardware_interface/include/fake_components/generic_system.hpp @@ -17,130 +17,13 @@ #ifndef FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ #define FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ -#include -#include -#include - -#include "hardware_interface/base_interface.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/hardware_interface_status_values.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" - -using hardware_interface::return_type; +#include "mock_components/generic_system.hpp" namespace fake_components { -enum StoppingInterface -{ - NONE, - STOP_POSITION, - STOP_VELOCITY -}; - -class HARDWARE_INTERFACE_PUBLIC GenericSystem -: public hardware_interface::BaseInterface -{ -public: - return_type configure(const hardware_interface::HardwareInfo & info) override; - - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - - return_type start() override - { - status_ = hardware_interface::status::STARTED; - return return_type::OK; - } - - return_type stop() override - { - status_ = hardware_interface::status::STOPPED; - return return_type::OK; - } - - return_type read() override; - - return_type write() override { return return_type::OK; } - - return_type prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) override; - - return_type perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) override; - -protected: - /// Use standard interfaces for joints because they are relevant for dynamic behaviour - /** - * By splitting the standard interfaces from other type, the users are able to inherit this - * class and simply create small "simulation" with desired dynamic behaviour. - * The advantage over using Gazebo is that enables "quick & dirty" tests of robot's URDF and - * controllers. - */ - const std::vector standard_interfaces_ = { - hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, - hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; - - const size_t POSITION_INTERFACE_INDEX = 0; - const size_t VELOCITY_INTERFACE_INDEX = 1; - - struct MimicJoint - { - std::size_t joint_index; - std::size_t mimicked_joint_index; - double multiplier = 1.0; - }; - std::vector mimic_joints_; - - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector> joint_commands_; - std::vector> joint_states_; - - std::vector other_interfaces_; - /// The size of this vector is (other_interfaces_.size() x nr_joints) - std::vector> other_commands_; - std::vector> other_states_; - - std::vector sensor_interfaces_; - /// The size of this vector is (other_interfaces_.size() x nr_joints) - std::vector> sensor_fake_commands_; - std::vector> sensor_states_; - -private: - template - bool get_interface( - const std::string & name, const std::vector & interface_list, - const std::string & interface_name, const size_t vector_index, - std::vector> & values, std::vector & interfaces); - - void initialize_storage_vectors( - std::vector> & commands, std::vector> & states, - const std::vector & interfaces); - - bool fake_sensor_command_interfaces_; - double position_state_following_offset_; - std::string custom_interface_with_following_offset_; - size_t index_custom_interface_with_following_offset_; - bool command_propagation_disabled_; - - // resources switching aux vars - std::vector stop_modes_; - std::vector start_modes_; - bool position_controller_running_; - bool velocity_controller_running_; - std::chrono::system_clock::time_point begin; - // for velocity control, store last position command - std::vector joint_pos_commands_old_; - double period_; -}; - -typedef GenericSystem GenericRobot; +using GenericSystem [[deprecated]] = mock_components::GenericSystem; +using GenericSystem [[deprecated]] = mock_components::GenericRobot; } // namespace fake_components #endif // FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_ diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index dacf71c461..e9b38de65d 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -91,12 +91,12 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector sensor_interfaces_; /// The size of this vector is (sensor_interfaces_.size() x nr_joints) - std::vector> sensor_fake_commands_; + std::vector> sensor_mock_commands_; std::vector> sensor_states_; std::vector gpio_interfaces_; /// The size of this vector is (gpio_interfaces_.size() x nr_joints) - std::vector> gpio_fake_commands_; + std::vector> gpio_mock_commands_; std::vector> gpio_commands_; std::vector> gpio_states_; @@ -118,7 +118,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste std::vector & interfaces, std::vector> & storage, std::vector & target_interfaces, bool using_state_interfaces); - bool use_fake_gpio_command_interfaces_; + bool use_mock_gpio_command_interfaces_; bool use_mock_sensor_command_interfaces_; double position_state_following_offset_; @@ -127,6 +127,8 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste bool calculate_dynamics_; std::vector joint_control_mode_; + + bool command_propagation_disabled_; }; typedef GenericSystem GenericRobot;