Skip to content

Commit 1187a90

Browse files
mamueluthmamueluth
committed
not finished, showcase for what hase to be changed
1 parent 0d97fa4 commit 1187a90

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
@@ -398,7 +398,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
398398

399399
double get_state(const std::string & interface_name) const
400400
{
401-
return actuator_states_.at(interface_name)->get_value();
401+
return actuator_states_.at(interface_name)->get_value<double>();
402402
}
403403

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

409409
double get_command(const std::string & interface_name) const
410410
{
411-
return actuator_commands_.at(interface_name)->get_value();
411+
return actuator_commands_.at(interface_name)->get_value<double>();
412412
}
413413

414-
void set_emergency_stop(const double & emergency_stop)
414+
void set_emergency_stop(const bool & emergency_stop)
415415
{
416416
emergency_stop_->set_value(emergency_stop);
417417
}
418418

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

421-
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
421+
void set_error_code(std::array<uint8_t, hardware_interface::error_signal_count> error_codes)
422+
{
423+
error_signal_->set_value(error_codes);
424+
}
422425

423-
double get_error_code() const { return error_signal_->get_value(); }
426+
std::array<uint8_t, hardware_interface::error_signal_count> get_error_code() const
427+
{
428+
return error_signal_->get_value<std::array<uint8_t, hardware_interface::error_signal_count>>();
429+
}
424430

425-
void set_error_message(const double & error_message)
431+
void set_error_message(
432+
std::array<std::string, hardware_interface::error_signal_count> error_messages)
426433
{
427-
error_signal_message_->set_value(error_message);
434+
error_signal_message_->set_value(error_messages);
428435
}
429436

430-
double get_error_message() const { return error_signal_message_->get_value(); }
437+
std::array<std::string, hardware_interface::error_signal_count> get_error_message() const
438+
{
439+
return error_signal_message_
440+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
441+
}
431442

432-
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
443+
void set_warning_code(std::array<int8_t, hardware_interface::warning_signal_count> warning_codes)
444+
{
445+
warning_signal_->set_value(warning_codes);
446+
}
433447

434-
double get_warning_code() const { return warning_signal_->get_value(); }
448+
std::array<int8_t, hardware_interface::warning_signal_count> get_warning_code() const
449+
{
450+
return warning_signal_
451+
->get_value<std::array<int8_t, hardware_interface::warning_signal_count>>();
452+
}
435453

436-
void set_warning_message(const double & error_message)
454+
void set_warning_message(
455+
std::array<std::string, hardware_interface::warning_signal_count> error_message)
437456
{
438457
warning_signal_message_->set_value(error_message);
439458
}
440459

441-
double get_warning_message() const { return warning_signal_message_->get_value(); }
460+
std::array<std::string, hardware_interface::error_signal_count> get_warning_message() const
461+
{
462+
return warning_signal_message_
463+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
464+
}
442465

443466
protected:
444467
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
@@ -265,30 +265,53 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
265265

266266
double get_state(const std::string & interface_name) const
267267
{
268-
return sensor_states_.at(interface_name)->get_value();
268+
return sensor_states_.at(interface_name)->get_value<double>();
269269
}
270270

271-
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
271+
void set_error_code(std::array<uint8_t, hardware_interface::error_signal_count> error_codes)
272+
{
273+
error_signal_->set_value(error_codes);
274+
}
272275

273-
double get_error_code() const { return error_signal_->get_value(); }
276+
std::array<uint8_t, hardware_interface::error_signal_count> get_error_code() const
277+
{
278+
return error_signal_->get_value<std::array<uint8_t, hardware_interface::error_signal_count>>();
279+
}
274280

275-
void set_error_message(const double & error_message)
281+
void set_error_message(
282+
std::array<std::string, hardware_interface::error_signal_count> error_messages)
276283
{
277-
error_signal_message_->set_value(error_message);
284+
error_signal_message_->set_value(error_messages);
278285
}
279286

280-
double get_error_message() const { return error_signal_message_->get_value(); }
287+
std::array<std::string, hardware_interface::error_signal_count> get_error_message() const
288+
{
289+
return error_signal_message_
290+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
291+
}
281292

282-
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
293+
void set_warning_code(std::array<int8_t, hardware_interface::warning_signal_count> warning_codes)
294+
{
295+
warning_signal_->set_value(warning_codes);
296+
}
283297

284-
double get_warning_code() const { return warning_signal_->get_value(); }
298+
std::array<int8_t, hardware_interface::warning_signal_count> get_warning_code() const
299+
{
300+
return warning_signal_
301+
->get_value<std::array<int8_t, hardware_interface::warning_signal_count>>();
302+
}
285303

286-
void set_warning_message(const double & error_message)
304+
void set_warning_message(
305+
std::array<std::string, hardware_interface::warning_signal_count> error_message)
287306
{
288307
warning_signal_message_->set_value(error_message);
289308
}
290309

291-
double get_warning_message() const { return warning_signal_message_->get_value(); }
310+
std::array<std::string, hardware_interface::error_signal_count> get_warning_message() const
311+
{
312+
return warning_signal_message_
313+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
314+
}
292315

293316
protected:
294317
HardwareInfo info_;

‎hardware_interface/include/hardware_interface/system_interface.hpp‎

Lines changed: 36 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -430,15 +430,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
430430
* \return state.
431431
*/
432432
void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }
433-
434433
void set_state(const std::string & interface_name, const double & value)
435434
{
436435
system_states_.at(interface_name)->set_value(value);
437436
}
438437

439438
double get_state(const std::string & interface_name) const
440439
{
441-
return system_states_.at(interface_name)->get_value();
440+
return system_states_.at(interface_name)->get_value<double>();
442441
}
443442

444443
void set_command(const std::string & interface_name, const double & value)
@@ -448,37 +447,60 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
448447

449448
double get_command(const std::string & interface_name) const
450449
{
451-
return system_commands_.at(interface_name)->get_value();
450+
return system_commands_.at(interface_name)->get_value<double>();
452451
}
453452

454-
void set_emergency_stop(const double & emergency_stop)
453+
void set_emergency_stop(const bool & emergency_stop)
455454
{
456455
emergency_stop_->set_value(emergency_stop);
457456
}
458457

459-
double get_emergency_stop() const { return emergency_stop_->get_value(); }
458+
bool get_emergency_stop() const { return emergency_stop_->get_value<bool>(); }
460459

461-
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
460+
void set_error_code(std::array<uint8_t, hardware_interface::error_signal_count> error_codes)
461+
{
462+
error_signal_->set_value(error_codes);
463+
}
462464

463-
double get_error_code() const { return error_signal_->get_value(); }
465+
std::array<uint8_t, hardware_interface::error_signal_count> get_error_code() const
466+
{
467+
return error_signal_->get_value<std::array<uint8_t, hardware_interface::error_signal_count>>();
468+
}
464469

465-
void set_error_message(const double & error_message)
470+
void set_error_message(
471+
std::array<std::string, hardware_interface::error_signal_count> error_messages)
466472
{
467-
error_signal_message_->set_value(error_message);
473+
error_signal_message_->set_value(error_messages);
468474
}
469475

470-
double get_error_message() const { return error_signal_message_->get_value(); }
476+
std::array<std::string, hardware_interface::error_signal_count> get_error_message() const
477+
{
478+
return error_signal_message_
479+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
480+
}
471481

472-
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
482+
void set_warning_code(std::array<int8_t, hardware_interface::warning_signal_count> warning_codes)
483+
{
484+
warning_signal_->set_value(warning_codes);
485+
}
473486

474-
double get_warning_code() const { return warning_signal_->get_value(); }
487+
std::array<int8_t, hardware_interface::warning_signal_count> get_warning_code() const
488+
{
489+
return warning_signal_
490+
->get_value<std::array<int8_t, hardware_interface::warning_signal_count>>();
491+
}
475492

476-
void set_warning_message(const double & error_message)
493+
void set_warning_message(
494+
std::array<std::string, hardware_interface::warning_signal_count> error_message)
477495
{
478496
warning_signal_message_->set_value(error_message);
479497
}
480498

481-
double get_warning_message() const { return warning_signal_message_->get_value(); }
499+
std::array<std::string, hardware_interface::error_signal_count> get_warning_message() const
500+
{
501+
return warning_signal_message_
502+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
503+
}
482504

483505
protected:
484506
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)