Skip to content

Commit 57da131

Browse files
saikishorsaikishor
committed
update the logic to use the clock to initialize the last read and write cycle time
1 parent 4d0bc78 commit 57da131

File tree

4 files changed

+16
-21
lines changed

4 files changed

+16
-21
lines changed

‎hardware_interface/src/actuator.cpp

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,8 @@ const rclcpp_lifecycle::State & Actuator::initialize(
4444
switch (impl_->init(actuator_info, logger, clock_interface))
4545
{
4646
case CallbackReturn::SUCCESS:
47+
last_read_cycle_time_ = clock_interface->get_clock()->now();
48+
last_write_cycle_time_ = clock_interface->get_clock()->now();
4749
impl_->set_state(rclcpp_lifecycle::State(
4850
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
4951
lifecycle_state_names::UNCONFIGURED));
@@ -229,7 +231,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p
229231
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
230232
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
231233
{
232-
last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type());
234+
last_read_cycle_time_ = time;
233235
return return_type::OK;
234236
}
235237
return_type result = return_type::ERROR;
@@ -253,7 +255,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration &
253255
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
254256
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
255257
{
256-
last_write_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type());
258+
last_write_cycle_time_ = time;
257259
return return_type::OK;
258260
}
259261
return_type result = return_type::ERROR;

‎hardware_interface/src/resource_manager.cpp

Lines changed: 6 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -1800,15 +1800,10 @@ HardwareReadWriteStatus ResourceManager::read(
18001800
const double read_rate =
18011801
resource_storage_->hardware_info_map_[component.get_name()].read_rate;
18021802
const auto current_time = resource_storage_->get_clock()->now();
1803-
if (
1804-
component.get_last_read_time().seconds() == 0 ||
1805-
(current_time - component.get_last_read_time()).seconds() * read_rate >= 0.99)
1803+
const rclcpp::Duration actual_period = current_time - component.get_last_read_time();
1804+
if (actual_period.seconds() * read_rate >= 0.99)
18061805
{
1807-
const rclcpp::Duration actual_period =
1808-
component.get_last_read_time().seconds() == 0
1809-
? rclcpp::Duration::from_seconds(1.0 / read_rate)
1810-
: current_time - component.get_last_read_time();
1811-
ret_val = component.read(time, actual_period);
1806+
ret_val = component.read(current_time, actual_period);
18121807
}
18131808
}
18141809
const auto component_group = component.get_group_name();
@@ -1882,15 +1877,10 @@ HardwareReadWriteStatus ResourceManager::write(
18821877
const double write_rate =
18831878
resource_storage_->hardware_info_map_[component.get_name()].write_rate;
18841879
const auto current_time = resource_storage_->get_clock()->now();
1885-
if (
1886-
component.get_last_write_time().seconds() == 0 ||
1887-
(current_time - component.get_last_write_time()).seconds() * write_rate >= 0.99)
1880+
const rclcpp::Duration actual_period = current_time - component.get_last_write_time();
1881+
if (actual_period.seconds() * write_rate >= 0.99)
18881882
{
1889-
const rclcpp::Duration actual_period =
1890-
component.get_last_write_time().seconds() == 0
1891-
? rclcpp::Duration::from_seconds(1.0 / write_rate)
1892-
: current_time - component.get_last_write_time();
1893-
ret_val = component.write(time, actual_period);
1883+
ret_val = component.write(current_time, actual_period);
18941884
}
18951885
}
18961886
const auto component_group = component.get_group_name();

‎hardware_interface/src/sensor.cpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,7 @@ const rclcpp_lifecycle::State & Sensor::initialize(
4343
switch (impl_->init(sensor_info, logger, clock_interface))
4444
{
4545
case CallbackReturn::SUCCESS:
46+
last_read_cycle_time_ = clock_interface->get_clock()->now();
4647
impl_->set_state(rclcpp_lifecycle::State(
4748
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
4849
lifecycle_state_names::UNCONFIGURED));
@@ -205,7 +206,7 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per
205206
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
206207
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
207208
{
208-
last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type());
209+
last_read_cycle_time_ = time;
209210
return return_type::OK;
210211
}
211212
return_type result = return_type::ERROR;

‎hardware_interface/src/system.cpp

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,8 @@ const rclcpp_lifecycle::State & System::initialize(
4343
switch (impl_->init(system_info, logger, clock_interface))
4444
{
4545
case CallbackReturn::SUCCESS:
46+
last_read_cycle_time_ = clock_interface->get_clock()->now();
47+
last_write_cycle_time_ = clock_interface->get_clock()->now();
4648
impl_->set_state(rclcpp_lifecycle::State(
4749
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
4850
lifecycle_state_names::UNCONFIGURED));
@@ -226,7 +228,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per
226228
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
227229
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
228230
{
229-
last_read_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type());
231+
last_read_cycle_time_ = time;
230232
return return_type::OK;
231233
}
232234
return_type result = return_type::ERROR;
@@ -250,7 +252,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe
250252
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED ||
251253
impl_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
252254
{
253-
last_write_cycle_time_ = rclcpp::Time(0, 0, time.get_clock_type());
255+
last_write_cycle_time_ = time;
254256
return return_type::OK;
255257
}
256258
return_type result = return_type::ERROR;

0 commit comments

Comments
 (0)