Skip to content

Commit c6176b5

Browse files
saikishorsaikishor
committed
first implementation of the serialized trigger
1 parent f49689e commit c6176b5

File tree

5 files changed

+94
-116
lines changed

5 files changed

+94
-116
lines changed

‎hardware_interface/include/hardware_interface/actuator_interface.hpp‎

Lines changed: 32 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -122,18 +122,25 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
122122
async_handler_->init(
123123
[this](const rclcpp::Time & time, const rclcpp::Duration & period)
124124
{
125-
if (next_trigger_ == TriggerType::READ)
125+
const auto read_start_time = std::chrono::steady_clock::now();
126+
const auto ret_read = read(time, period);
127+
const auto read_end_time = std::chrono::steady_clock::now();
128+
read_return_info_.store(ret_read, std::memory_order_release);
129+
read_execution_time_.store(
130+
std::chrono::duration_cast<std::chrono::nanoseconds>(read_end_time - read_start_time),
131+
std::memory_order_release);
132+
if (ret_read != return_type::OK)
126133
{
127-
const auto ret = read(time, period);
128-
next_trigger_.store(TriggerType::WRITE, std::memory_order_release);
129-
return ret;
130-
}
131-
else
132-
{
133-
const auto ret = write(time, period);
134-
next_trigger_.store(TriggerType::READ, std::memory_order_release);
135-
return ret;
134+
return ret_read;
136135
}
136+
const auto write_start_time = std::chrono::steady_clock::now();
137+
const auto ret_write = write(time, period);
138+
const auto write_end_time = std::chrono::steady_clock::now();
139+
write_return_info_.store(ret_write, std::memory_order_release);
140+
write_execution_time_.store(
141+
std::chrono::duration_cast<std::chrono::nanoseconds>(write_end_time - write_start_time),
142+
std::memory_order_release);
143+
return ret_write;
137144
},
138145
info_.thread_priority);
139146
async_handler_->start_thread();
@@ -366,30 +373,20 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
366373
status.result = return_type::ERROR;
367374
if (info_.is_async)
368375
{
369-
if (next_trigger_.load(std::memory_order_acquire) == TriggerType::WRITE)
376+
status.result = read_return_info_.load(std::memory_order_acquire);
377+
const auto read_exec_time = read_execution_time_.load(std::memory_order_acquire);
378+
if (read_exec_time.count() > 0)
370379
{
371-
RCLCPP_WARN(
372-
get_logger(),
373-
"Trigger read called while write async handler call is still pending for hardware "
374-
"interface : '%s'. Skipping read cycle and will wait for a write cycle!",
375-
info_.name.c_str());
376-
status.result = return_type::OK;
377-
return status;
380+
status.execution_time = read_exec_time;
378381
}
379382
const auto result = async_handler_->trigger_async_callback(time, period);
380383
status.successful = result.first;
381-
status.result = result.second;
382-
const auto execution_time = async_handler_->get_last_execution_time();
383-
if (execution_time.count() > 0)
384-
{
385-
status.execution_time = execution_time;
386-
}
387384
if (!status.successful)
388385
{
389386
RCLCPP_WARN(
390387
get_logger(),
391-
"Trigger read called while write async trigger is still in progress for hardware "
392-
"interface : '%s'. Failed to trigger read cycle!",
388+
"Trigger read/write called while the previous async trigger is still in progress for "
389+
"hardware interface : '%s'. Failed to trigger read/write cycle!",
393390
info_.name.c_str());
394391
status.result = return_type::OK;
395392
return status;
@@ -435,34 +432,13 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
435432
status.result = return_type::ERROR;
436433
if (info_.is_async)
437434
{
438-
if (next_trigger_.load(std::memory_order_acquire) == TriggerType::READ)
439-
{
440-
RCLCPP_WARN(
441-
get_logger(),
442-
"Trigger write called while read async handler call is still pending for hardware "
443-
"interface : '%s'. Skipping write cycle and will wait for a read cycle!",
444-
info_.name.c_str());
445-
status.result = return_type::OK;
446-
return status;
447-
}
448-
const auto result = async_handler_->trigger_async_callback(time, period);
449-
status.successful = result.first;
450-
status.result = result.second;
451-
const auto execution_time = async_handler_->get_last_execution_time();
452-
if (execution_time.count() > 0)
453-
{
454-
status.execution_time = execution_time;
455-
}
456-
if (!status.successful)
435+
status.successful = true;
436+
const auto write_exec_time = write_execution_time_.load(std::memory_order_acquire);
437+
if (write_exec_time.count() > 0)
457438
{
458-
RCLCPP_WARN(
459-
get_logger(),
460-
"Trigger write called while read async trigger is still in progress for hardware "
461-
"interface : '%s'. Failed to trigger write cycle!",
462-
info_.name.c_str());
463-
status.result = return_type::OK;
464-
return status;
439+
status.execution_time = write_exec_time;
465440
}
441+
status.result = write_return_info_.load(std::memory_order_acquire);
466442
}
467443
else
468444
{
@@ -592,7 +568,10 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
592568
// interface names to Handle accessed through getters/setters
593569
std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;
594570
std::unordered_map<std::string, CommandInterface::SharedPtr> actuator_commands_;
595-
std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
571+
std::atomic<return_type> read_return_info_;
572+
std::atomic<std::chrono::nanoseconds> read_execution_time_ = std::chrono::nanoseconds::zero();
573+
std::atomic<return_type> write_return_info_;
574+
std::atomic<std::chrono::nanoseconds> write_execution_time_ = std::chrono::nanoseconds::zero();
596575

597576
protected:
598577
pal_statistics::RegistrationsRAII stats_registrations_;

‎hardware_interface/include/hardware_interface/system_interface.hpp‎

Lines changed: 32 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -125,18 +125,25 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
125125
async_handler_->init(
126126
[this](const rclcpp::Time & time, const rclcpp::Duration & period)
127127
{
128-
if (next_trigger_ == TriggerType::READ)
128+
const auto read_start_time = std::chrono::steady_clock::now();
129+
const auto ret_read = read(time, period);
130+
const auto read_end_time = std::chrono::steady_clock::now();
131+
read_return_info_.store(ret_read, std::memory_order_release);
132+
read_execution_time_.store(
133+
std::chrono::duration_cast<std::chrono::nanoseconds>(read_end_time - read_start_time),
134+
std::memory_order_release);
135+
if (ret_read != return_type::OK)
129136
{
130-
const auto ret = read(time, period);
131-
next_trigger_.store(TriggerType::WRITE, std::memory_order_release);
132-
return ret;
133-
}
134-
else
135-
{
136-
const auto ret = write(time, period);
137-
next_trigger_.store(TriggerType::READ, std::memory_order_release);
138-
return ret;
137+
return ret_read;
139138
}
139+
const auto write_start_time = std::chrono::steady_clock::now();
140+
const auto ret_write = write(time, period);
141+
const auto write_end_time = std::chrono::steady_clock::now();
142+
write_return_info_.store(ret_write, std::memory_order_release);
143+
write_execution_time_.store(
144+
std::chrono::duration_cast<std::chrono::nanoseconds>(write_end_time - write_start_time),
145+
std::memory_order_release);
146+
return ret_write;
140147
},
141148
info_.thread_priority);
142149
async_handler_->start_thread();
@@ -395,30 +402,20 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
395402
status.result = return_type::ERROR;
396403
if (info_.is_async)
397404
{
398-
if (next_trigger_.load(std::memory_order_acquire) == TriggerType::WRITE)
405+
status.result = read_return_info_.load(std::memory_order_acquire);
406+
const auto read_exec_time = read_execution_time_.load(std::memory_order_acquire);
407+
if (read_exec_time.count() > 0)
399408
{
400-
RCLCPP_WARN(
401-
get_logger(),
402-
"Trigger read called while write async handler call is still pending for hardware "
403-
"interface : '%s'. Skipping read cycle and will wait for a write cycle!",
404-
info_.name.c_str());
405-
status.result = return_type::OK;
406-
return status;
409+
status.execution_time = read_exec_time;
407410
}
408411
const auto result = async_handler_->trigger_async_callback(time, period);
409412
status.successful = result.first;
410-
status.result = result.second;
411-
const auto execution_time = async_handler_->get_last_execution_time();
412-
if (execution_time.count() > 0)
413-
{
414-
status.execution_time = execution_time;
415-
}
416413
if (!status.successful)
417414
{
418415
RCLCPP_WARN(
419416
get_logger(),
420-
"Trigger read called while write async trigger is still in progress for hardware "
421-
"interface : '%s'. Failed to trigger read cycle!",
417+
"Trigger read/write called while the previous async trigger is still in progress for "
418+
"hardware interface : '%s'. Failed to trigger read/write cycle!",
422419
info_.name.c_str());
423420
status.result = return_type::OK;
424421
return status;
@@ -464,34 +461,13 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
464461
status.result = return_type::ERROR;
465462
if (info_.is_async)
466463
{
467-
if (next_trigger_.load(std::memory_order_acquire) == TriggerType::READ)
468-
{
469-
RCLCPP_WARN(
470-
get_logger(),
471-
"Trigger write called while read async handler call is still pending for hardware "
472-
"interface : '%s'. Skipping write cycle and will wait for a read cycle!",
473-
info_.name.c_str());
474-
status.result = return_type::OK;
475-
return status;
476-
}
477-
const auto result = async_handler_->trigger_async_callback(time, period);
478-
status.successful = result.first;
479-
status.result = result.second;
480-
const auto execution_time = async_handler_->get_last_execution_time();
481-
if (execution_time.count() > 0)
482-
{
483-
status.execution_time = execution_time;
484-
}
485-
if (!status.successful)
464+
status.successful = true;
465+
const auto write_exec_time = write_execution_time_.load(std::memory_order_acquire);
466+
if (write_exec_time.count() > 0)
486467
{
487-
RCLCPP_WARN(
488-
get_logger(),
489-
"Trigger write called while read async trigger is still in progress for hardware "
490-
"interface : '%s'. Failed to trigger write cycle!",
491-
info_.name.c_str());
492-
status.result = return_type::OK;
493-
return status;
468+
status.execution_time = write_exec_time;
494469
}
470+
status.result = write_return_info_.load(std::memory_order_acquire);
495471
}
496472
else
497473
{
@@ -631,7 +607,10 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
631607
// interface names to Handle accessed through getters/setters
632608
std::unordered_map<std::string, StateInterface::SharedPtr> system_states_;
633609
std::unordered_map<std::string, CommandInterface::SharedPtr> system_commands_;
634-
std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
610+
std::atomic<return_type> read_return_info_;
611+
std::atomic<std::chrono::nanoseconds> read_execution_time_ = std::chrono::nanoseconds::zero();
612+
std::atomic<return_type> write_return_info_;
613+
std::atomic<std::chrono::nanoseconds> write_execution_time_ = std::chrono::nanoseconds::zero();
635614

636615
protected:
637616
pal_statistics::RegistrationsRAII stats_registrations_;

‎hardware_interface_testing/test/test_components/test_actuator.cpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -111,6 +111,11 @@ class TestActuator : public ActuatorInterface
111111

112112
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
113113
{
114+
if (get_hardware_info().is_async)
115+
{
116+
std::this_thread::sleep_for(
117+
std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate)));
118+
}
114119
// simulate error on read
115120
if (velocity_command_ == test_constants::READ_FAIL_VALUE)
116121
{
@@ -135,6 +140,11 @@ class TestActuator : public ActuatorInterface
135140

136141
return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
137142
{
143+
if (get_hardware_info().is_async)
144+
{
145+
std::this_thread::sleep_for(
146+
std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate)));
147+
}
138148
// simulate error on write
139149
if (velocity_command_ == test_constants::WRITE_FAIL_VALUE)
140150
{

‎hardware_interface_testing/test/test_components/test_system.cpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -100,6 +100,11 @@ class TestSystem : public SystemInterface
100100

101101
return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
102102
{
103+
if (get_hardware_info().is_async)
104+
{
105+
std::this_thread::sleep_for(
106+
std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate)));
107+
}
103108
// simulate error on read
104109
if (velocity_command_[0] == test_constants::READ_FAIL_VALUE)
105110
{
@@ -124,6 +129,11 @@ class TestSystem : public SystemInterface
124129

125130
return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
126131
{
132+
if (get_hardware_info().is_async)
133+
{
134+
std::this_thread::sleep_for(
135+
std::chrono::milliseconds(1000 / (3 * get_hardware_info().rw_rate)));
136+
}
127137
// simulate error on write
128138
if (velocity_command_[0] == test_constants::WRITE_FAIL_VALUE)
129139
{

‎hardware_interface_testing/test/test_resource_manager.cpp‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -2118,7 +2118,6 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest
21182118
auto [ok, failed_hardware_names] = rm->read(time, duration);
21192119
EXPECT_TRUE(ok);
21202120
EXPECT_TRUE(failed_hardware_names.empty());
2121-
std::this_thread::sleep_for(std::chrono::milliseconds(1));
21222121
// The values are computations exactly within the test_components
21232122
if (check_for_updated_values)
21242123
{
@@ -2134,7 +2133,6 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest
21342133
ASSERT_NEAR(
21352134
state_itfs[1].get_optional().value(), prev_system_state_value, system_increment / 2.0);
21362135
auto [ok_write, failed_hardware_names_write] = rm->write(time, duration);
2137-
std::this_thread::sleep_for(std::chrono::milliseconds(1));
21382136
EXPECT_TRUE(ok_write);
21392137
EXPECT_TRUE(failed_hardware_names_write.empty());
21402138
node_.get_clock()->sleep_until(time + duration);
@@ -2149,7 +2147,7 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest
21492147
1.2 * rate);
21502148
EXPECT_THAT(
21512149
status_map[component_name].read_statistics->periodicity.get_statistics().min,
2152-
testing::AllOf(testing::Ge(0.5 * rate), testing::Lt((1.2 * rate))));
2150+
testing::AllOf(testing::Ge(0.4 * rate), testing::Lt((1.2 * rate))));
21532151
EXPECT_THAT(
21542152
status_map[component_name].read_statistics->periodicity.get_statistics().max,
21552153
testing::AllOf(testing::Ge(0.75 * rate), testing::Lt((2.0 * rate))));
@@ -2159,36 +2157,38 @@ class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest
21592157
1.2 * rate);
21602158
EXPECT_THAT(
21612159
status_map[component_name].write_statistics->periodicity.get_statistics().min,
2162-
testing::AllOf(testing::Ge(0.5 * rate), testing::Lt((1.2 * rate))));
2160+
testing::AllOf(testing::Ge(0.4 * rate), testing::Lt((1.2 * rate))));
21632161
EXPECT_THAT(
21642162
status_map[component_name].write_statistics->periodicity.get_statistics().max,
21652163
testing::AllOf(testing::Ge(0.75 * rate), testing::Lt((2.0 * rate))));
21662164
};
21672165

21682166
if (check_for_updated_values)
21692167
{
2170-
check_periodicity(TEST_ACTUATOR_HARDWARE_NAME, 100u);
2171-
check_periodicity(TEST_SYSTEM_HARDWARE_NAME, 100u);
2168+
const unsigned int rw_rate = 100u;
2169+
const double expec_execution_time = (1.e6 / (3 * rw_rate)) + 200.0;
2170+
check_periodicity(TEST_ACTUATOR_HARDWARE_NAME, rw_rate);
2171+
check_periodicity(TEST_SYSTEM_HARDWARE_NAME, rw_rate);
21722172
EXPECT_LT(
21732173
status_map[TEST_ACTUATOR_HARDWARE_NAME]
21742174
.read_statistics->execution_time.get_statistics()
21752175
.average,
2176-
100);
2176+
expec_execution_time);
21772177
EXPECT_LT(
21782178
status_map[TEST_ACTUATOR_HARDWARE_NAME]
21792179
.write_statistics->execution_time.get_statistics()
21802180
.average,
2181-
100);
2181+
expec_execution_time);
21822182
EXPECT_LT(
21832183
status_map[TEST_SYSTEM_HARDWARE_NAME]
21842184
.read_statistics->execution_time.get_statistics()
21852185
.average,
2186-
100);
2186+
expec_execution_time);
21872187
EXPECT_LT(
21882188
status_map[TEST_SYSTEM_HARDWARE_NAME]
21892189
.write_statistics->execution_time.get_statistics()
21902190
.average,
2191-
100);
2191+
expec_execution_time);
21922192
}
21932193
}
21942194

0 commit comments

Comments
 (0)