Skip to content

Commit

Permalink
tmp commit! remove deprecated handle constructor -> adjust generic sy…
Browse files Browse the repository at this point in the history 
…stem

! not compiling not all tests have been adjusted still wip!
  • Loading branch information
@mamueluth
mamueluth committed Apr 11, 2024
1 parent c3c8718 commit ad416ce
Show file tree
Hide file tree
Showing 16 changed files with 406 additions and 609 deletions.
32 changes: 5 additions & 27 deletions hardware_interface/include/hardware_interface/handle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,13 +36,12 @@ class Handle
public:
[[deprecated("Use InterfaceDescription for initializing the Interface")]]

Handle(
const std::string & prefix_name, const std::string & interface_name,
double * value_ptr = nullptr)
: prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr)
Handle(const std::string & prefix_name, const std::string & interface_name)
: prefix_name_(prefix_name), interface_name_(interface_name)
{
// default to double
value_ = std::numeric_limits<double>::quiet_NaN();
// init to default value defined by init_handle_value()
InterfaceInfo info;
init_handle_value(info);
}

explicit Handle(const InterfaceDescription & interface_description)
Expand All @@ -52,23 +51,6 @@ class Handle
init_handle_value(interface_description.interface_info);
}

[[deprecated("Use InterfaceDescription for initializing the Interface")]]

explicit Handle(const std::string & interface_name)
: interface_name_(interface_name), value_ptr_(nullptr)
{
// default to double
value_ = std::numeric_limits<double>::quiet_NaN();
}

[[deprecated("Use InterfaceDescription for initializing the Interface")]]

explicit Handle(const char * interface_name)
: interface_name_(interface_name), value_ptr_(nullptr)
{ // default to double
value_ = std::numeric_limits<double>::quiet_NaN();
}

Handle(const Handle & other) = default;

Handle(Handle && other) = default;
Expand All @@ -79,9 +61,6 @@ class Handle

virtual ~Handle() = default;

/// Returns true if handle references a value.
inline operator bool() const { return value_ptr_ != nullptr; }

const std::string get_name() const { return prefix_name_ + "/" + interface_name_; }

const std::string & get_interface_name() const { return interface_name_; }
Expand Down Expand Up @@ -183,7 +162,6 @@ class Handle
std::string prefix_name_;
std::string interface_name_;
HANDLE_DATATYPE value_;
double * value_ptr_;
};

class StateInterface : public Handle
Expand Down
67 changes: 28 additions & 39 deletions hardware_interface/include/mock_components/generic_system.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,8 @@
#define MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_

#include <string>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include "hardware_interface/handle.hpp"
Expand All @@ -41,9 +43,7 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste
public:
CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;

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

std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
std::vector<hardware_interface::InterfaceDescription> export_command_interfaces_2() override;

return_type prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
Expand Down Expand Up @@ -71,54 +71,43 @@ class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::Syste
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};
// added dynamically during on_init
std::vector<std::string> non_standard_interfaces_;

/// 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 (sensor_interfaces_.size() x nr_joints)
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_mock_commands_;
std::vector<std::vector<double>> gpio_commands_;
std::vector<std::vector<double>> gpio_states_;
struct MimicJoint
{
std::string joint_name;
std::string mimic_joint_name;
double multiplier = 1.0;
};
std::vector<MimicJoint> mimic_joints_;

// All the joints that are of type defined by standard_interfaces_ vector -> In {pos, vel, acc,
// effort}
std::vector<std::string> std_joint_names_;
std::unordered_set<std::string> std_joint_command_interface_names_;
std::unordered_set<std::string> std_joint_state_interface_names_;

// All the joints that are of not of a type defined by standard_interfaces_ vector -> Not in {pos,
// vel, acc, effort}
std::vector<std::string> other_joint_names_;
std::unordered_set<std::string> other_joint_command_interface_names_;
std::unordered_set<std::string> other_joint_state_interface_names_;

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,
const std::vector<hardware_interface::ComponentInfo> & component_infos);

template <typename InterfaceType>
bool populate_interfaces(
void search_and_add_interface_names(
const std::vector<hardware_interface::ComponentInfo> & components,
std::vector<std::string> & interfaces, std::vector<std::vector<double>> & storage,
std::vector<InterfaceType> & target_interfaces, bool using_state_interfaces);
const std::vector<std::string> & interface_list, std::vector<std::string> & vector_to_add);

bool use_mock_gpio_command_interfaces_;
bool use_mock_sensor_command_interfaces_;

double position_state_following_offset_;
std::string custom_interface_with_following_offset_;
size_t index_custom_interface_with_following_offset_;
std::string custom_interface_name_with_following_offset_;

bool calculate_dynamics_;
std::vector<size_t> joint_control_mode_;
std::unordered_map<std::string, size_t> joint_control_mode_;

bool command_propagation_disabled_;
};
Expand Down
Loading

0 comments on commit ad416ce

Please sign in to comment.