Skip to content

Commit 6a3ac65

Browse files
mamueluthmamueluth
committed
not finished, showcase for what hase to be changed
1 parent 53b4c1d commit 6a3ac65

16 files changed

+424
-300
lines changed

‎controller_interface/test/test_chainable_controller_interface.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
4242
EXPECT_EQ(reference_interfaces[0].get_prefix_name(), TEST_CONTROLLER_NAME);
4343
EXPECT_EQ(reference_interfaces[0].get_interface_name(), "test_itf");
4444

45-
EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE);
45+
EXPECT_EQ(reference_interfaces[0].get_value<double>(), INTERFACE_VALUE);
4646
}
4747

4848
TEST_F(ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size)
@@ -88,7 +88,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
8888
EXPECT_FALSE(controller.is_in_chained_mode());
8989

9090
// Fail setting chained mode
91-
EXPECT_EQ(reference_interfaces[0].get_value(), INTERFACE_VALUE);
91+
EXPECT_EQ(reference_interfaces[0].get_value<double>(), INTERFACE_VALUE);
9292

9393
EXPECT_FALSE(controller.set_chained_mode(true));
9494
EXPECT_FALSE(controller.is_in_chained_mode());

‎hardware_interface/include/hardware_interface/actuator_interface.hpp‎

Lines changed: 36 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -431,7 +431,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
431431

432432
double get_state(const std::string & interface_name) const
433433
{
434-
return actuator_states_.at(interface_name)->get_value();
434+
return actuator_states_.at(interface_name)->get_value<double>();
435435
}
436436

437437
void set_command(const std::string & interface_name, const double & value)
@@ -441,37 +441,60 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
441441

442442
double get_command(const std::string & interface_name) const
443443
{
444-
return actuator_commands_.at(interface_name)->get_value();
444+
return actuator_commands_.at(interface_name)->get_value<double>();
445445
}
446446

447-
void set_emergency_stop(const double & emergency_stop)
447+
void set_emergency_stop(const bool & emergency_stop)
448448
{
449449
emergency_stop_->set_value(emergency_stop);
450450
}
451451

452-
double get_emergency_stop() const { return emergency_stop_->get_value(); }
452+
bool get_emergency_stop() const { return emergency_stop_->get_value<bool>(); }
453453

454-
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
454+
void set_error_code(std::array<uint8_t, hardware_interface::error_signal_count> error_codes)
455+
{
456+
error_signal_->set_value(error_codes);
457+
}
455458

456-
double get_error_code() const { return error_signal_->get_value(); }
459+
std::array<uint8_t, hardware_interface::error_signal_count> get_error_code() const
460+
{
461+
return error_signal_->get_value<std::array<uint8_t, hardware_interface::error_signal_count>>();
462+
}
457463

458-
void set_error_message(const double & error_message)
464+
void set_error_message(
465+
std::array<std::string, hardware_interface::error_signal_count> error_messages)
459466
{
460-
error_signal_message_->set_value(error_message);
467+
error_signal_message_->set_value(error_messages);
461468
}
462469

463-
double get_error_message() const { return error_signal_message_->get_value(); }
470+
std::array<std::string, hardware_interface::error_signal_count> get_error_message() const
471+
{
472+
return error_signal_message_
473+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
474+
}
464475

465-
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
476+
void set_warning_code(std::array<int8_t, hardware_interface::warning_signal_count> warning_codes)
477+
{
478+
warning_signal_->set_value(warning_codes);
479+
}
466480

467-
double get_warning_code() const { return warning_signal_->get_value(); }
481+
std::array<int8_t, hardware_interface::warning_signal_count> get_warning_code() const
482+
{
483+
return warning_signal_
484+
->get_value<std::array<int8_t, hardware_interface::warning_signal_count>>();
485+
}
468486

469-
void set_warning_message(const double & error_message)
487+
void set_warning_message(
488+
std::array<std::string, hardware_interface::warning_signal_count> error_message)
470489
{
471490
warning_signal_message_->set_value(error_message);
472491
}
473492

474-
double get_warning_message() const { return warning_signal_message_->get_value(); }
493+
std::array<std::string, hardware_interface::error_signal_count> get_warning_message() const
494+
{
495+
return warning_signal_message_
496+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
497+
}
475498

476499
protected:
477500
HardwareInfo info_;

‎hardware_interface/include/hardware_interface/handle.hpp‎

Lines changed: 22 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <limits>
2020
#include <memory>
2121
#include <string>
22+
#include <type_traits>
2223
#include <utility>
2324
#include <variant>
2425

@@ -30,7 +31,19 @@
3031
namespace hardware_interface
3132
{
3233

33-
typedef std::variant<double> HANDLE_DATATYPE;
34+
using HANDLE_DATATYPE = std::variant<
35+
bool, double, hardware_interface::WARNING_SIGNALS, hardware_interface::ERROR_SIGNALS,
36+
hardware_interface::WARNING_MESSAGES>;
37+
38+
// Define a type trait for allowed types
39+
template <typename T>
40+
struct HANDLE_DATATYPE_TYPES : std::disjunction<
41+
std::is_same<T, bool>, std::is_same<T, double>,
42+
std::is_same<T, hardware_interface::WARNING_SIGNALS>,
43+
std::is_same<T, hardware_interface::ERROR_SIGNALS>,
44+
std::is_same<T, hardware_interface::WARNING_MESSAGES>>
45+
{
46+
};
3447

3548
/// A handle used to get and set a value on a given interface.
3649
class Handle
@@ -49,10 +62,10 @@ class Handle
4962
: prefix_name_(interface_description.prefix_name),
5063
interface_name_(interface_description.interface_info.name)
5164
{
65+
// TODO(Manuel): implement this.
5266
// As soon as multiple datatypes are used in HANDLE_DATATYPE
5367
// we need to initialize according the type passed in interface description
54-
value_ = std::numeric_limits<double>::quiet_NaN();
55-
value_ptr_ = std::get_if<double>(&value_);
68+
// value_ = std::numeric_limits<double>::quiet_NaN();
5669
}
5770

5871
[[deprecated("Use InterfaceDescription for initializing the Interface")]]
@@ -95,32 +108,23 @@ class Handle
95108

96109
const std::string & get_prefix_name() const { return prefix_name_; }
97110

98-
double get_value() const
111+
template <typename T, typename std::enable_if<HANDLE_DATATYPE_TYPES<T>::value, int>::type = 0>
112+
T get_value() const
99113
{
100-
// BEGIN (Handle export change): for backward compatibility
101-
// TODO(Manuel) return value_ if old functionality is removed
102-
THROW_ON_NULLPTR(value_ptr_);
103-
return *value_ptr_;
104-
// END
114+
return std::get<T>(value_);
105115
}
106116

107-
void set_value(double value)
117+
template <typename T, typename std::enable_if<HANDLE_DATATYPE_TYPES<T>::value, int>::type = 0>
118+
void set_value(T value)
108119
{
109-
// BEGIN (Handle export change): for backward compatibility
110-
// TODO(Manuel) set value_ directly if old functionality is removed
111-
THROW_ON_NULLPTR(this->value_ptr_);
112-
*this->value_ptr_ = value;
113-
// END
120+
value_ = value;
114121
}
115122

116123
protected:
117124
std::string prefix_name_;
118125
std::string interface_name_;
119126
HANDLE_DATATYPE value_;
120-
// BEGIN (Handle export change): for backward compatibility
121-
// TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed
122127
double * value_ptr_;
123-
// END
124128
};
125129

126130
class StateInterface : public Handle

‎hardware_interface/include/hardware_interface/loaned_command_interface.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ class LoanedCommandInterface
6565

6666
void set_value(double val) { command_interface_.set_value(val); }
6767

68-
double get_value() const { return command_interface_.get_value(); }
68+
double get_value() const { return command_interface_.get_value<double>(); }
6969

7070
protected:
7171
CommandInterface & command_interface_;

‎hardware_interface/include/hardware_interface/loaned_state_interface.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ class LoanedStateInterface
6363

6464
const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); }
6565

66-
double get_value() const { return state_interface_.get_value(); }
66+
double get_value() const { return state_interface_.get_value<double>(); }
6767

6868
protected:
6969
StateInterface & state_interface_;

‎hardware_interface/include/hardware_interface/sensor_interface.hpp‎

Lines changed: 33 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -282,30 +282,53 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
282282

283283
double get_state(const std::string & interface_name) const
284284
{
285-
return sensor_states_.at(interface_name)->get_value();
285+
return sensor_states_.at(interface_name)->get_value<double>();
286286
}
287287

288-
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
288+
void set_error_code(std::array<uint8_t, hardware_interface::error_signal_count> error_codes)
289+
{
290+
error_signal_->set_value(error_codes);
291+
}
289292

290-
double get_error_code() const { return error_signal_->get_value(); }
293+
std::array<uint8_t, hardware_interface::error_signal_count> get_error_code() const
294+
{
295+
return error_signal_->get_value<std::array<uint8_t, hardware_interface::error_signal_count>>();
296+
}
291297

292-
void set_error_message(const double & error_message)
298+
void set_error_message(
299+
std::array<std::string, hardware_interface::error_signal_count> error_messages)
293300
{
294-
error_signal_message_->set_value(error_message);
301+
error_signal_message_->set_value(error_messages);
295302
}
296303

297-
double get_error_message() const { return error_signal_message_->get_value(); }
304+
std::array<std::string, hardware_interface::error_signal_count> get_error_message() const
305+
{
306+
return error_signal_message_
307+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
308+
}
298309

299-
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
310+
void set_warning_code(std::array<int8_t, hardware_interface::warning_signal_count> warning_codes)
311+
{
312+
warning_signal_->set_value(warning_codes);
313+
}
300314

301-
double get_warning_code() const { return warning_signal_->get_value(); }
315+
std::array<int8_t, hardware_interface::warning_signal_count> get_warning_code() const
316+
{
317+
return warning_signal_
318+
->get_value<std::array<int8_t, hardware_interface::warning_signal_count>>();
319+
}
302320

303-
void set_warning_message(const double & error_message)
321+
void set_warning_message(
322+
std::array<std::string, hardware_interface::warning_signal_count> error_message)
304323
{
305324
warning_signal_message_->set_value(error_message);
306325
}
307326

308-
double get_warning_message() const { return warning_signal_message_->get_value(); }
327+
std::array<std::string, hardware_interface::error_signal_count> get_warning_message() const
328+
{
329+
return warning_signal_message_
330+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
331+
}
309332

310333
protected:
311334
HardwareInfo info_;

‎hardware_interface/include/hardware_interface/system_interface.hpp‎

Lines changed: 36 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -463,15 +463,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
463463
* \return state.
464464
*/
465465
void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }
466-
467466
void set_state(const std::string & interface_name, const double & value)
468467
{
469468
system_states_.at(interface_name)->set_value(value);
470469
}
471470

472471
double get_state(const std::string & interface_name) const
473472
{
474-
return system_states_.at(interface_name)->get_value();
473+
return system_states_.at(interface_name)->get_value<double>();
475474
}
476475

477476
void set_command(const std::string & interface_name, const double & value)
@@ -481,37 +480,60 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
481480

482481
double get_command(const std::string & interface_name) const
483482
{
484-
return system_commands_.at(interface_name)->get_value();
483+
return system_commands_.at(interface_name)->get_value<double>();
485484
}
486485

487-
void set_emergency_stop(const double & emergency_stop)
486+
void set_emergency_stop(const bool & emergency_stop)
488487
{
489488
emergency_stop_->set_value(emergency_stop);
490489
}
491490

492-
double get_emergency_stop() const { return emergency_stop_->get_value(); }
491+
bool get_emergency_stop() const { return emergency_stop_->get_value<bool>(); }
493492

494-
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
493+
void set_error_code(std::array<uint8_t, hardware_interface::error_signal_count> error_codes)
494+
{
495+
error_signal_->set_value(error_codes);
496+
}
495497

496-
double get_error_code() const { return error_signal_->get_value(); }
498+
std::array<uint8_t, hardware_interface::error_signal_count> get_error_code() const
499+
{
500+
return error_signal_->get_value<std::array<uint8_t, hardware_interface::error_signal_count>>();
501+
}
497502

498-
void set_error_message(const double & error_message)
503+
void set_error_message(
504+
std::array<std::string, hardware_interface::error_signal_count> error_messages)
499505
{
500-
error_signal_message_->set_value(error_message);
506+
error_signal_message_->set_value(error_messages);
501507
}
502508

503-
double get_error_message() const { return error_signal_message_->get_value(); }
509+
std::array<std::string, hardware_interface::error_signal_count> get_error_message() const
510+
{
511+
return error_signal_message_
512+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
513+
}
504514

505-
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
515+
void set_warning_code(std::array<int8_t, hardware_interface::warning_signal_count> warning_codes)
516+
{
517+
warning_signal_->set_value(warning_codes);
518+
}
506519

507-
double get_warning_code() const { return warning_signal_->get_value(); }
520+
std::array<int8_t, hardware_interface::warning_signal_count> get_warning_code() const
521+
{
522+
return warning_signal_
523+
->get_value<std::array<int8_t, hardware_interface::warning_signal_count>>();
524+
}
508525

509-
void set_warning_message(const double & error_message)
526+
void set_warning_message(
527+
std::array<std::string, hardware_interface::warning_signal_count> error_message)
510528
{
511529
warning_signal_message_->set_value(error_message);
512530
}
513531

514-
double get_warning_message() const { return warning_signal_message_->get_value(); }
532+
std::array<std::string, hardware_interface::error_signal_count> get_warning_message() const
533+
{
534+
return warning_signal_message_
535+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
536+
}
515537

516538
protected:
517539
HardwareInfo info_;

‎hardware_interface/include/hardware_interface/types/hardware_interface_error_signals.hpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,13 +16,15 @@
1616
#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_ERROR_SIGNALS_HPP_
1717

1818
#include <cstdint>
19+
#include <string>
1920
#include <vector>
2021

2122
namespace hardware_interface
2223
{
2324

2425
// Count of how many different error signals there are that can be reported.
2526
const size_t error_signal_count = 32;
27+
using ERROR_SIGNALS = std::array<uint8_t, hardware_interface::error_signal_count>;
2628

2729
constexpr char ERROR_SIGNAL_INTERFACE_NAME[] = "ERROR_SIGNAL";
2830
// Available error signal names
@@ -62,6 +64,8 @@ enum class error_signal : std::uint8_t
6264
ERROR_SIGNAL_31 = 31
6365
};
6466

67+
using ERROR_MESSAGES = std::array<std::string, hardware_interface::error_signal_count>;
68+
6569
constexpr char ERROR_SIGNAL_MESSAGE_INTERFACE_NAME[] = "ERROR_SIGNAL_MESSAGE";
6670
// Available WARNING signal message names
6771
enum class error_signal_message : std::uint8_t

0 commit comments

Comments
 (0)