Skip to content

Commit

Permalink
Sync with the last functional state.
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl committed Jun 15, 2023
1 parent 65f7a54 commit 3fa2570
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 123 deletions.
123 changes: 3 additions & 120 deletions hardware_interface/include/fake_components/generic_system.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,130 +17,13 @@
#ifndef FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_
#define FAKE_COMPONENTS__GENERIC_SYSTEM_HPP_

#include <chrono>
#include <string>
#include <vector>

#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<hardware_interface::SystemInterface>
{
public:
return_type configure(const hardware_interface::HardwareInfo & info) override;

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

std::vector<hardware_interface::CommandInterface> 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<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces) override;

return_type perform_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & 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<std::string> 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<MimicJoint> mimic_joints_;

/// The size of this vector is (standard_interfaces_.size() x nr_joints)
std::vector<std::vector<double>> joint_commands_;
std::vector<std::vector<double>> joint_states_;

std::vector<std::string> other_interfaces_;
/// The size of this vector is (other_interfaces_.size() x nr_joints)
std::vector<std::vector<double>> other_commands_;
std::vector<std::vector<double>> other_states_;

std::vector<std::string> sensor_interfaces_;
/// The size of this vector is (other_interfaces_.size() x nr_joints)
std::vector<std::vector<double>> sensor_fake_commands_;
std::vector<std::vector<double>> sensor_states_;

private:
template <typename HandleType>
bool get_interface(
const std::string & name, const std::vector<std::string> & interface_list,
const std::string & interface_name, const size_t vector_index,
std::vector<std::vector<double>> & values, std::vector<HandleType> & interfaces);

void initialize_storage_vectors(
std::vector<std::vector<double>> & commands, std::vector<std::vector<double>> & states,
const std::vector<std::string> & 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<uint> stop_modes_;
std::vector<std::string> 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<double> 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_
8 changes: 5 additions & 3 deletions hardware_interface/include/mock_components/generic_system.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,12 +91,12 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste

std::vector<std::string> sensor_interfaces_;
/// The size of this vector is (sensor_interfaces_.size() x nr_joints)
std::vector<std::vector<double>> sensor_fake_commands_;
std::vector<std::vector<double>> sensor_mock_commands_;
std::vector<std::vector<double>> sensor_states_;

std::vector<std::string> gpio_interfaces_;
/// The size of this vector is (gpio_interfaces_.size() x nr_joints)
std::vector<std::vector<double>> gpio_fake_commands_;
std::vector<std::vector<double>> gpio_mock_commands_;
std::vector<std::vector<double>> gpio_commands_;
std::vector<std::vector<double>> gpio_states_;

Expand All @@ -118,7 +118,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste
std::vector<std::string> & interfaces, std::vector<std::vector<double>> & storage,
std::vector<InterfaceType> & 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_;
Expand All @@ -127,6 +127,8 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste

bool calculate_dynamics_;
std::vector<size_t> joint_control_mode_;

bool command_propagation_disabled_;
};

typedef GenericSystem GenericRobot;
Expand Down

0 comments on commit 3fa2570

Please sign in to comment.