Skip to content

Commit 726bca7

Browse files
mamueluthmamueluth
committed
not finished, showcase for what hase to be changed
1 parent ee6ae9e commit 726bca7

16 files changed

+428
-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
@@ -370,7 +370,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
370370

371371
double get_state(const std::string & interface_name) const
372372
{
373-
return actuator_states_.at(interface_name)->get_value();
373+
return actuator_states_.at(interface_name)->get_value<double>();
374374
}
375375

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

381381
double get_command(const std::string & interface_name) const
382382
{
383-
return actuator_commands_.at(interface_name)->get_value();
383+
return actuator_commands_.at(interface_name)->get_value<double>();
384384
}
385385

386-
void set_emergency_stop(const double & emergency_stop)
386+
void set_emergency_stop(const bool & emergency_stop)
387387
{
388388
emergency_stop_->set_value(emergency_stop);
389389
}
390390

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

393-
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
393+
void set_error_code(std::array<uint8_t, hardware_interface::error_signal_count> error_codes)
394+
{
395+
error_signal_->set_value(error_codes);
396+
}
394397

395-
double get_error_code() const { return error_signal_->get_value(); }
398+
std::array<uint8_t, hardware_interface::error_signal_count> get_error_code() const
399+
{
400+
return error_signal_->get_value<std::array<uint8_t, hardware_interface::error_signal_count>>();
401+
}
396402

397-
void set_error_message(const double & error_message)
403+
void set_error_message(
404+
std::array<std::string, hardware_interface::error_signal_count> error_messages)
398405
{
399-
error_signal_message_->set_value(error_message);
406+
error_signal_message_->set_value(error_messages);
400407
}
401408

402-
double get_error_message() const { return error_signal_message_->get_value(); }
409+
std::array<std::string, hardware_interface::error_signal_count> get_error_message() const
410+
{
411+
return error_signal_message_
412+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
413+
}
403414

404-
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
415+
void set_warning_code(std::array<int8_t, hardware_interface::warning_signal_count> warning_codes)
416+
{
417+
warning_signal_->set_value(warning_codes);
418+
}
405419

406-
double get_warning_code() const { return warning_signal_->get_value(); }
420+
std::array<int8_t, hardware_interface::warning_signal_count> get_warning_code() const
421+
{
422+
return warning_signal_
423+
->get_value<std::array<int8_t, hardware_interface::warning_signal_count>>();
424+
}
407425

408-
void set_warning_message(const double & error_message)
426+
void set_warning_message(
427+
std::array<std::string, hardware_interface::warning_signal_count> error_message)
409428
{
410429
warning_signal_message_->set_value(error_message);
411430
}
412431

413-
double get_warning_message() const { return warning_signal_message_->get_value(); }
432+
std::array<std::string, hardware_interface::error_signal_count> get_warning_message() const
433+
{
434+
return warning_signal_message_
435+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
436+
}
414437

415438
protected:
416439
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
@@ -252,30 +252,53 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
252252

253253
double get_state(const std::string & interface_name) const
254254
{
255-
return sensor_states_.at(interface_name)->get_value();
255+
return sensor_states_.at(interface_name)->get_value<double>();
256256
}
257257

258-
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
258+
void set_error_code(std::array<uint8_t, hardware_interface::error_signal_count> error_codes)
259+
{
260+
error_signal_->set_value(error_codes);
261+
}
259262

260-
double get_error_code() const { return error_signal_->get_value(); }
263+
std::array<uint8_t, hardware_interface::error_signal_count> get_error_code() const
264+
{
265+
return error_signal_->get_value<std::array<uint8_t, hardware_interface::error_signal_count>>();
266+
}
261267

262-
void set_error_message(const double & error_message)
268+
void set_error_message(
269+
std::array<std::string, hardware_interface::error_signal_count> error_messages)
263270
{
264-
error_signal_message_->set_value(error_message);
271+
error_signal_message_->set_value(error_messages);
265272
}
266273

267-
double get_error_message() const { return error_signal_message_->get_value(); }
274+
std::array<std::string, hardware_interface::error_signal_count> get_error_message() const
275+
{
276+
return error_signal_message_
277+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
278+
}
268279

269-
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
280+
void set_warning_code(std::array<int8_t, hardware_interface::warning_signal_count> warning_codes)
281+
{
282+
warning_signal_->set_value(warning_codes);
283+
}
270284

271-
double get_warning_code() const { return warning_signal_->get_value(); }
285+
std::array<int8_t, hardware_interface::warning_signal_count> get_warning_code() const
286+
{
287+
return warning_signal_
288+
->get_value<std::array<int8_t, hardware_interface::warning_signal_count>>();
289+
}
272290

273-
void set_warning_message(const double & error_message)
291+
void set_warning_message(
292+
std::array<std::string, hardware_interface::warning_signal_count> error_message)
274293
{
275294
warning_signal_message_->set_value(error_message);
276295
}
277296

278-
double get_warning_message() const { return warning_signal_message_->get_value(); }
297+
std::array<std::string, hardware_interface::error_signal_count> get_warning_message() const
298+
{
299+
return warning_signal_message_
300+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
301+
}
279302

280303
protected:
281304
HardwareInfo info_;

‎hardware_interface/include/hardware_interface/system_interface.hpp‎

Lines changed: 36 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -402,15 +402,14 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
402402
* \return state.
403403
*/
404404
void set_state(const rclcpp_lifecycle::State & new_state) { lifecycle_state_ = new_state; }
405-
406405
void set_state(const std::string & interface_name, const double & value)
407406
{
408407
system_states_.at(interface_name)->set_value(value);
409408
}
410409

411410
double get_state(const std::string & interface_name) const
412411
{
413-
return system_states_.at(interface_name)->get_value();
412+
return system_states_.at(interface_name)->get_value<double>();
414413
}
415414

416415
void set_command(const std::string & interface_name, const double & value)
@@ -420,37 +419,60 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
420419

421420
double get_command(const std::string & interface_name) const
422421
{
423-
return system_commands_.at(interface_name)->get_value();
422+
return system_commands_.at(interface_name)->get_value<double>();
424423
}
425424

426-
void set_emergency_stop(const double & emergency_stop)
425+
void set_emergency_stop(const bool & emergency_stop)
427426
{
428427
emergency_stop_->set_value(emergency_stop);
429428
}
430429

431-
double get_emergency_stop() const { return emergency_stop_->get_value(); }
430+
bool get_emergency_stop() const { return emergency_stop_->get_value<bool>(); }
432431

433-
void set_error_code(const double & error_code) { error_signal_->set_value(error_code); }
432+
void set_error_code(std::array<uint8_t, hardware_interface::error_signal_count> error_codes)
433+
{
434+
error_signal_->set_value(error_codes);
435+
}
434436

435-
double get_error_code() const { return error_signal_->get_value(); }
437+
std::array<uint8_t, hardware_interface::error_signal_count> get_error_code() const
438+
{
439+
return error_signal_->get_value<std::array<uint8_t, hardware_interface::error_signal_count>>();
440+
}
436441

437-
void set_error_message(const double & error_message)
442+
void set_error_message(
443+
std::array<std::string, hardware_interface::error_signal_count> error_messages)
438444
{
439-
error_signal_message_->set_value(error_message);
445+
error_signal_message_->set_value(error_messages);
440446
}
441447

442-
double get_error_message() const { return error_signal_message_->get_value(); }
448+
std::array<std::string, hardware_interface::error_signal_count> get_error_message() const
449+
{
450+
return error_signal_message_
451+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
452+
}
443453

444-
void set_warning_code(const double & warning_codes) { warning_signal_->set_value(warning_codes); }
454+
void set_warning_code(std::array<int8_t, hardware_interface::warning_signal_count> warning_codes)
455+
{
456+
warning_signal_->set_value(warning_codes);
457+
}
445458

446-
double get_warning_code() const { return warning_signal_->get_value(); }
459+
std::array<int8_t, hardware_interface::warning_signal_count> get_warning_code() const
460+
{
461+
return warning_signal_
462+
->get_value<std::array<int8_t, hardware_interface::warning_signal_count>>();
463+
}
447464

448-
void set_warning_message(const double & error_message)
465+
void set_warning_message(
466+
std::array<std::string, hardware_interface::warning_signal_count> error_message)
449467
{
450468
warning_signal_message_->set_value(error_message);
451469
}
452470

453-
double get_warning_message() const { return warning_signal_message_->get_value(); }
471+
std::array<std::string, hardware_interface::error_signal_count> get_warning_message() const
472+
{
473+
return warning_signal_message_
474+
->get_value<std::array<std::string, hardware_interface::error_signal_count>>();
475+
}
454476

455477
protected:
456478
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)