Skip to content

Commit

Permalink
update the logic to use only single async_handler for both read and w…
Browse files Browse the repository at this point in the history 
…rite cycles
  • Loading branch information
@saikishor
saikishor committed Sep 26, 2024
1 parent 445bd19 commit 1f5e72e
Show file tree
Hide file tree
Showing 3 changed files with 59 additions and 79 deletions.
68 changes: 29 additions & 39 deletions hardware_interface/include/hardware_interface/actuator_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,14 +114,24 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
info_ = hardware_info;
if (info_.is_async)
{
read_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
write_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
read_async_handler_->init(
std::bind(&ActuatorInterface::read, this, std::placeholders::_1, std::placeholders::_2));
read_async_handler_->start_thread();
write_async_handler_->init(
std::bind(&ActuatorInterface::write, this, std::placeholders::_1, std::placeholders::_2));
write_async_handler_->start_thread();
async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
async_handler_->init(
[this](const rclcpp::Time & time, const rclcpp::Duration & period)
{
if (next_trigger_ == TriggerType::READ)
{
const auto ret = read(time, period);
next_trigger_ = TriggerType::WRITE;
return ret;
}
else
{
const auto ret = write(time, period);
next_trigger_ = TriggerType::READ;
return ret;
}
});
async_handler_->start_thread();
}
return on_init(hardware_info);
};
Expand Down Expand Up @@ -381,32 +391,22 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
if (next_trigger_ == TriggerType::WRITE)
{
RCLCPP_WARN(
rclcpp::get_logger("ActuatorInterface"),
get_logger(),
"Trigger read called while write async handler call is still pending for hardware "
"interface : '%s'. Skipping read cycle and will wait for a write cycle!",
info_.name.c_str());
return return_type::OK;
}
if (write_async_handler_->is_trigger_cycle_in_progress())
{
RCLCPP_WARN(
rclcpp::get_logger("ActuatorInterface"),
"Trigger read called while write async handler is still in progress for hardware "
"interface : '%s'. Skipping read cycle!",
info_.name.c_str());
return return_type::OK;
}
std::tie(trigger_status, result) = read_async_handler_->trigger_async_callback(time, period);
std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
if (!trigger_status)
{
RCLCPP_WARN(
rclcpp::get_logger("ActuatorInterface"),
"Trigger read called while read async handler is still in progress for hardware "
get_logger(),
"Trigger read called while write async trigger is still in progress for hardware "
"interface : '%s'. Failed to trigger read cycle!",
info_.name.c_str());
return return_type::OK;
}
next_trigger_ = TriggerType::WRITE;
}
else
{
Expand Down Expand Up @@ -446,32 +446,22 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
if (next_trigger_ == TriggerType::READ)
{
RCLCPP_WARN(
rclcpp::get_logger("ActuatorInterface"),
get_logger(),
"Trigger write called while read async handler call is still pending for hardware "
"interface : '%s'. Skipping write cycle and will wait for a read cycle!",
info_.name.c_str());
return return_type::OK;
}
if (read_async_handler_->is_trigger_cycle_in_progress())
{
RCLCPP_WARN(
rclcpp::get_logger("ActuatorInterface"),
"Trigger write called while read async handler is still in progress for hardware "
"interface : '%s'. Skipping write cycle!",
info_.name.c_str());
return return_type::OK;
}
std::tie(trigger_status, result) = write_async_handler_->trigger_async_callback(time, period);
std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
if (!trigger_status)
{
RCLCPP_WARN(
rclcpp::get_logger("ActuatorInterface"),
"Trigger write called while write async handler is still in progress for hardware "
get_logger(),
"Trigger write called while read async trigger is still in progress for hardware "
"interface : '%s'. Failed to trigger write cycle!",
info_.name.c_str());
return return_type::OK;
}
next_trigger_ = TriggerType::READ;
}
else
{
Expand Down Expand Up @@ -573,8 +563,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
std::vector<CommandInterface::SharedPtr> unlisted_commands_;

rclcpp_lifecycle::State lifecycle_state_;
std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> read_async_handler_;
std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> write_async_handler_;
std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;

private:
rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
Expand All @@ -586,7 +575,8 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
{
READ,
WRITE
} next_trigger_ = TriggerType::READ;
};
std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
};

} // namespace hardware_interface
Expand Down
2 changes: 1 addition & 1 deletion hardware_interface/include/hardware_interface/sensor_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -246,7 +246,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
if (!trigger_status)
{
RCLCPP_WARN(
rclcpp::get_logger("SensorInterface"),
get_logger(),
"Trigger read called while read async handler is still in progress for hardware "
"interface : '%s'. Failed to trigger read cycle!",
info_.name.c_str());
Expand Down
68 changes: 29 additions & 39 deletions hardware_interface/include/hardware_interface/system_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,14 +117,24 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
info_ = hardware_info;
if (info_.is_async)
{
read_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
write_async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
read_async_handler_->init(
std::bind(&SystemInterface::read, this, std::placeholders::_1, std::placeholders::_2));
read_async_handler_->start_thread();
write_async_handler_->init(
std::bind(&SystemInterface::write, this, std::placeholders::_1, std::placeholders::_2));
write_async_handler_->start_thread();
async_handler_ = std::make_unique<realtime_tools::AsyncFunctionHandler<return_type>>();
async_handler_->init(
[this](const rclcpp::Time & time, const rclcpp::Duration & period)
{
if (next_trigger_ == TriggerType::READ)
{
const auto ret = read(time, period);
next_trigger_ = TriggerType::WRITE;
return ret;
}
else
{
const auto ret = write(time, period);
next_trigger_ = TriggerType::READ;
return ret;
}
});
async_handler_->start_thread();
}
return on_init(hardware_info);
};
Expand Down Expand Up @@ -425,32 +435,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
if (next_trigger_ == TriggerType::WRITE)
{
RCLCPP_WARN(
rclcpp::get_logger("SystemInterface"),
get_logger(),
"Trigger read called while write async handler call is still pending for hardware "
"interface : '%s'. Skipping read cycle and will wait for a write cycle!",
info_.name.c_str());
return return_type::OK;
}
if (write_async_handler_->is_trigger_cycle_in_progress())
{
RCLCPP_WARN(
rclcpp::get_logger("SystemInterface"),
"Trigger read called while write async handler is still in progress for hardware "
"interface : '%s'. Skipping read cycle!",
info_.name.c_str());
return return_type::OK;
}
std::tie(trigger_status, result) = read_async_handler_->trigger_async_callback(time, period);
std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
if (!trigger_status)
{
RCLCPP_WARN(
rclcpp::get_logger("SystemInterface"),
"Trigger read called while read async handler is still in progress for hardware "
get_logger(),
"Trigger read called while write async trigger is still in progress for hardware "
"interface : '%s'. Failed to trigger read cycle!",
info_.name.c_str());
return return_type::OK;
}
next_trigger_ = TriggerType::WRITE;
}
else
{
Expand Down Expand Up @@ -490,32 +490,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
if (next_trigger_ == TriggerType::READ)
{
RCLCPP_WARN(
rclcpp::get_logger("SystemInterface"),
get_logger(),
"Trigger write called while read async handler call is still pending for hardware "
"interface : '%s'. Skipping write cycle and will wait for a read cycle!",
info_.name.c_str());
return return_type::OK;
}
if (read_async_handler_->is_trigger_cycle_in_progress())
{
RCLCPP_WARN(
rclcpp::get_logger("SystemInterface"),
"Trigger write called while read async handler is still in progress for hardware "
"interface : '%s'. Skipping write cycle!",
info_.name.c_str());
return return_type::OK;
}
std::tie(trigger_status, result) = write_async_handler_->trigger_async_callback(time, period);
std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period);
if (!trigger_status)
{
RCLCPP_WARN(
rclcpp::get_logger("SystemInterface"),
"Trigger write called while write async handler is still in progress for hardware "
get_logger(),
"Trigger write called while read async trigger is still in progress for hardware "
"interface : '%s'. Failed to trigger write cycle!",
info_.name.c_str());
return return_type::OK;
}
next_trigger_ = TriggerType::READ;
}
else
{
Expand Down Expand Up @@ -615,8 +605,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
std::unordered_map<std::string, InterfaceDescription> unlisted_command_interfaces_;

rclcpp_lifecycle::State lifecycle_state_;
std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> read_async_handler_;
std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> write_async_handler_;
std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;

// Exported Command- and StateInterfaces in order they are listed in the hardware description.
std::vector<StateInterface::SharedPtr> joint_states_;
Expand All @@ -640,7 +629,8 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
{
READ,
WRITE
} next_trigger_ = TriggerType::READ;
};
std::atomic<TriggerType> next_trigger_ = TriggerType::READ;
};

} // namespace hardware_interface
Expand Down

0 comments on commit 1f5e72e

Please sign in to comment.