Skip to content

Commit

Permalink
Fix the logic in the hardware components periodicity and proper cycles
Browse files Browse the repository at this point in the history
  • Loading branch information
@saikishor
saikishor committed Jan 19, 2025
1 parent 4a67663 commit 8a64061
Show file tree
Hide file tree
Showing 4 changed files with 85 additions and 56 deletions.
48 changes: 28 additions & 20 deletions hardware_interface/src/actuator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,8 @@ Actuator::Actuator(Actuator && other) noexcept
{
std::lock_guard<std::recursive_mutex> lock(other.actuators_mutex_);
impl_ = std::move(other.impl_);
last_read_cycle_time_ = other.last_read_cycle_time_;
last_write_cycle_time_ = other.last_write_cycle_time_;
last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
}

const rclcpp_lifecycle::State & Actuator::initialize(
Expand All @@ -55,8 +55,6 @@ const rclcpp_lifecycle::State & Actuator::initialize(
switch (impl_->init(actuator_info, logger, clock_interface))
{
case CallbackReturn::SUCCESS:
last_read_cycle_time_ = clock_interface->get_clock()->now();
last_write_cycle_time_ = clock_interface->get_clock()->now();
impl_->set_lifecycle_state(rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED));
Expand Down Expand Up @@ -141,6 +139,8 @@ const rclcpp_lifecycle::State & Actuator::shutdown()
const rclcpp_lifecycle::State & Actuator::activate()
{
std::unique_lock<std::recursive_mutex> lock(actuators_mutex_);
last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
read_statistics_.reset_statistics();
write_statistics_.reset_statistics();
if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
Expand Down Expand Up @@ -318,16 +318,20 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p
{
error();
}
if (trigger_result.successful && trigger_result.execution_time.has_value())
if (trigger_result.successful)
{
read_statistics_.execution_time->AddMeasurement(
static_cast<double>(trigger_result.execution_time.value().count()) / 1.e3);
if (trigger_result.execution_time.has_value())
{
read_statistics_.execution_time->AddMeasurement(
static_cast<double>(trigger_result.execution_time.value().count()) / 1.e3);
}
if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED)
{
read_statistics_.periodicity->AddMeasurement(
1.0 / (time - last_read_cycle_time_).seconds());
}
last_read_cycle_time_ = time;
}
if (trigger_result.successful && trigger_result.period.has_value())
{
read_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds());
}
last_read_cycle_time_ = time;
return trigger_result.result;
}
return return_type::OK;
Expand All @@ -349,16 +353,20 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration &
{
error();
}
if (trigger_result.successful && trigger_result.execution_time.has_value())
if (trigger_result.successful)
{
write_statistics_.execution_time->AddMeasurement(
static_cast<double>(trigger_result.execution_time.value().count()) / 1.e3);
if (trigger_result.execution_time.has_value())
{
write_statistics_.execution_time->AddMeasurement(
static_cast<double>(trigger_result.execution_time.value().count()) / 1.e3);
}
if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED)
{
write_statistics_.periodicity->AddMeasurement(
1.0 / (time - last_write_cycle_time_).seconds());
}
last_write_cycle_time_ = time;
}
if (trigger_result.successful && trigger_result.period.has_value())
{
write_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds());
}
last_write_cycle_time_ = time;
return trigger_result.result;
}
return return_type::OK;
Expand Down
21 changes: 15 additions & 6 deletions hardware_interface/src/resource_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1758,17 +1758,22 @@ HardwareReadWriteStatus ResourceManager::read(
{
auto & hardware_component_info =
resource_storage_->hardware_info_map_[component.get_name()];
const auto current_time = resource_storage_->get_clock()->now();
const rclcpp::Duration actual_period =
component.get_last_read_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED
? current_time - component.get_last_read_time()
: rclcpp::Duration::from_seconds(
1.0 / static_cast<double>(hardware_component_info.rw_rate));
// const rclcpp::Duration actual_period = current_time - component.get_last_read_time();
if (
hardware_component_info.rw_rate == 0 ||
hardware_component_info.rw_rate == resource_storage_->cm_update_rate_)
{
ret_val = component.read(time, period);
ret_val = component.read(current_time, actual_period);
}
else
{
const double read_rate = hardware_component_info.rw_rate;
const auto current_time = resource_storage_->get_clock()->now();
const rclcpp::Duration actual_period = current_time - component.get_last_read_time();
if (actual_period.seconds() * read_rate >= 0.99)
{
ret_val = component.read(current_time, actual_period);
Expand Down Expand Up @@ -1852,17 +1857,21 @@ HardwareReadWriteStatus ResourceManager::write(
{
auto & hardware_component_info =
resource_storage_->hardware_info_map_[component.get_name()];
const auto current_time = resource_storage_->get_clock()->now();
const rclcpp::Duration actual_period =
component.get_last_write_time().get_clock_type() != RCL_CLOCK_UNINITIALIZED
? current_time - component.get_last_write_time()
: rclcpp::Duration::from_seconds(
1.0 / static_cast<double>(hardware_component_info.rw_rate));
if (
hardware_component_info.rw_rate == 0 ||
hardware_component_info.rw_rate == resource_storage_->cm_update_rate_)
{
ret_val = component.write(time, period);
ret_val = component.write(current_time, actual_period);
}
else
{
const double write_rate = hardware_component_info.rw_rate;
const auto current_time = resource_storage_->get_clock()->now();
const rclcpp::Duration actual_period = current_time - component.get_last_write_time();
if (actual_period.seconds() * write_rate >= 0.99)
{
ret_val = component.write(current_time, actual_period);
Expand Down
24 changes: 14 additions & 10 deletions hardware_interface/src/sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ Sensor::Sensor(Sensor && other) noexcept
{
std::lock_guard<std::recursive_mutex> lock(other.sensors_mutex_);
impl_ = std::move(other.impl_);
last_read_cycle_time_ = other.last_read_cycle_time_;
last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
}

const rclcpp_lifecycle::State & Sensor::initialize(
Expand All @@ -53,7 +53,6 @@ const rclcpp_lifecycle::State & Sensor::initialize(
switch (impl_->init(sensor_info, logger, clock_interface))
{
case CallbackReturn::SUCCESS:
last_read_cycle_time_ = clock_interface->get_clock()->now();
impl_->set_lifecycle_state(rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED));
Expand Down Expand Up @@ -138,6 +137,7 @@ const rclcpp_lifecycle::State & Sensor::shutdown()
const rclcpp_lifecycle::State & Sensor::activate()
{
std::unique_lock<std::recursive_mutex> lock(sensors_mutex_);
last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
read_statistics_.reset_statistics();
if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
Expand Down Expand Up @@ -266,16 +266,20 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per
{
error();
}
if (trigger_result.successful && trigger_result.execution_time.has_value())
if (trigger_result.successful)
{
read_statistics_.execution_time->AddMeasurement(
static_cast<double>(trigger_result.execution_time.value().count()) / 1.e3);
if (trigger_result.execution_time.has_value())
{
read_statistics_.execution_time->AddMeasurement(
static_cast<double>(trigger_result.execution_time.value().count()) / 1.e3);
}
if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED)
{
read_statistics_.periodicity->AddMeasurement(
1.0 / (time - last_read_cycle_time_).seconds());
}
last_read_cycle_time_ = time;
}
if (trigger_result.successful && trigger_result.period.has_value())
{
read_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds());
}
last_read_cycle_time_ = time;
return trigger_result.result;
}
return return_type::OK;
Expand Down
48 changes: 28 additions & 20 deletions hardware_interface/src/system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,8 @@ System::System(System && other) noexcept
{
std::lock_guard<std::recursive_mutex> lock(other.system_mutex_);
impl_ = std::move(other.impl_);
last_read_cycle_time_ = other.last_read_cycle_time_;
last_write_cycle_time_ = other.last_write_cycle_time_;
last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
}

const rclcpp_lifecycle::State & System::initialize(
Expand All @@ -53,8 +53,6 @@ const rclcpp_lifecycle::State & System::initialize(
switch (impl_->init(system_info, logger, clock_interface))
{
case CallbackReturn::SUCCESS:
last_read_cycle_time_ = clock_interface->get_clock()->now();
last_write_cycle_time_ = clock_interface->get_clock()->now();
impl_->set_lifecycle_state(rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED));
Expand Down Expand Up @@ -139,6 +137,8 @@ const rclcpp_lifecycle::State & System::shutdown()
const rclcpp_lifecycle::State & System::activate()
{
std::unique_lock<std::recursive_mutex> lock(system_mutex_);
last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
read_statistics_.reset_statistics();
write_statistics_.reset_statistics();
if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
Expand Down Expand Up @@ -316,16 +316,20 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per
{
error();
}
if (trigger_result.successful && trigger_result.execution_time.has_value())
if (trigger_result.successful)
{
read_statistics_.execution_time->AddMeasurement(
static_cast<double>(trigger_result.execution_time.value().count()) / 1.e3);
if (trigger_result.execution_time.has_value())
{
read_statistics_.execution_time->AddMeasurement(
static_cast<double>(trigger_result.execution_time.value().count()) / 1.e3);
}
if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED)
{
read_statistics_.periodicity->AddMeasurement(
1.0 / (time - last_read_cycle_time_).seconds());
}
last_read_cycle_time_ = time;
}
if (trigger_result.successful && trigger_result.period.has_value())
{
read_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds());
}
last_read_cycle_time_ = time;
return trigger_result.result;
}
return return_type::OK;
Expand All @@ -347,16 +351,20 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe
{
error();
}
if (trigger_result.successful && trigger_result.execution_time.has_value())
if (trigger_result.successful)
{
write_statistics_.execution_time->AddMeasurement(
static_cast<double>(trigger_result.execution_time.value().count()) / 1.e3);
if (trigger_result.execution_time.has_value())
{
write_statistics_.execution_time->AddMeasurement(
static_cast<double>(trigger_result.execution_time.value().count()) / 1.e3);
}
if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED)
{
write_statistics_.periodicity->AddMeasurement(
1.0 / (time - last_write_cycle_time_).seconds());
}
last_write_cycle_time_ = time;
}
if (trigger_result.successful && trigger_result.period.has_value())
{
write_statistics_.periodicity->AddMeasurement(1.0 / trigger_result.period.value().seconds());
}
last_write_cycle_time_ = time;
return trigger_result.result;
}
return return_type::OK;
Expand Down

0 comments on commit 8a64061

Please sign in to comment.