Skip to content

Commit

Permalink
[pre-commit.ci] auto fixes from pre-commit.com hooks
Browse files Browse the repository at this point in the history 
for more information, see https://pre-commit.ci
  • Loading branch information
@pre-commit-ci @olivier-stasse
pre-commit-ci[bot] authored and olivier-stasse committed Jul 21, 2024
1 parent 909fc6b commit 9ecf73f
Show file tree
Hide file tree
Showing 5 changed files with 78 additions and 82 deletions.
4 changes: 2 additions & 2 deletions README.md
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
# ODRI Gazebo ros2_control plugin
# ODRI Gazebo ros2_control plugin

This plug is a fork of [gz_ros2_control](https://github.com/ros-controls/gz_ros2_control).
It implements the specificities of ODRI that are accessible through ros2_control.
Expand Down Expand Up @@ -27,5 +27,5 @@ Then the target is to try to maintain this package for:
# Branches

master: Targets rolling releases and for now jammy and noble
jazzy: target
jazzy: target
humble:
8 changes: 4 additions & 4 deletions include/gz_ros2_control/gz_system.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,10 +74,10 @@ class GazeboOdriSimSystem : public GazeboOdriSimSystemInterface {
const rclcpp::Time& time, const rclcpp::Duration& period) override;

// Documentation Inherited
bool initSim(rclcpp::Node::SharedPtr &model_nh,
std::map<std::string, sim::Entity> &joints,
const hardware_interface::HardwareInfo &hardware_info,
sim::EntityComponentManager &_ecm,
bool initSim(rclcpp::Node::SharedPtr& model_nh,
std::map<std::string, sim::Entity>& joints,
const hardware_interface::HardwareInfo& hardware_info,
sim::EntityComponentManager& _ecm,
unsigned int update_rate) override;

private:
Expand Down
16 changes: 8 additions & 8 deletions include/gz_ros2_control/gz_system_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,9 +57,9 @@ class SafeEnum {
public:
SafeEnum() : mFlags(0) {}
explicit SafeEnum(ENUM singleFlag) : mFlags(singleFlag) {}
SafeEnum(const SafeEnum& original) : mFlags(original.mFlags) {}
SafeEnum(const SafeEnum &original) : mFlags(original.mFlags) {}

SafeEnum& operator|=(ENUM addValue) {
SafeEnum &operator|=(ENUM addValue) {
mFlags |= addValue;
return *this;
}
Expand All @@ -68,7 +68,7 @@ class SafeEnum {
result |= addValue;
return result;
}
SafeEnum& operator&=(ENUM maskValue) {
SafeEnum &operator&=(ENUM maskValue) {
mFlags &= maskValue;
return *this;
}
Expand Down Expand Up @@ -99,11 +99,11 @@ class GazeboOdriSimSystemInterface
/// is related with the entity in Gazebo param[in] hardware_info structure
/// with data from URDF. param[in] _ecm Entity-component manager. param[in]
/// update_rate controller update rate
virtual bool initSim(rclcpp::Node::SharedPtr &model_nh,
std::map<std::string, sim::Entity> &joints,
const hardware_interface::HardwareInfo &hardware_info,
sim::EntityComponentManager &_ecm,
unsigned int update_rate) = 0;
virtual bool initSim(rclcpp::Node::SharedPtr &model_nh,
std::map<std::string, sim::Entity> &joints,
const hardware_interface::HardwareInfo &hardware_info,
sim::EntityComponentManager &_ecm,
unsigned int update_rate) = 0;

// Methods used to control a joint.
enum ControlMethod_ {
Expand Down
127 changes: 62 additions & 65 deletions src/gz_ros2_control_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,70 +52,65 @@
#include "gz_ros2_control/gz_system.hpp"

namespace odri_gz_ros2_control {
class GZResourceManager : public hardware_interface::ResourceManager
{
public:
GZResourceManager(
rclcpp::Node::SharedPtr & node,
sim::EntityComponentManager & ecm,
std::map<std::string, sim::Entity> enabledJoints)
: hardware_interface::ResourceManager(
node->get_node_clock_interface(), node->get_node_logging_interface()),
gz_system_loader_("gz_ros2_control", "gz_ros2_control::GazeboOdriSimSystemInterface"),
logger_(node->get_logger().get_child("GZResourceManager"))
{
class GZResourceManager : public hardware_interface::ResourceManager {
public:
GZResourceManager(rclcpp::Node::SharedPtr &node,
sim::EntityComponentManager &ecm,
std::map<std::string, sim::Entity> enabledJoints)
: hardware_interface::ResourceManager(node->get_node_clock_interface(),
node->get_node_logging_interface()),
gz_system_loader_("gz_ros2_control",
"gz_ros2_control::GazeboOdriSimSystemInterface"),
logger_(node->get_logger().get_child("GZResourceManager")) {
node_ = node;
ecm_ = &ecm;
enabledJoints_ = enabledJoints;
}

GZResourceManager(const GZResourceManager &) = delete;

// Called from Controller Manager when robot description is initialized from callback
bool load_and_initialize_components(
const std::string & urdf,
unsigned int update_rate) override
{
// Called from Controller Manager when robot description is initialized from
// callback
bool load_and_initialize_components(const std::string &urdf,
unsigned int update_rate) override {
components_are_loaded_and_initialized_ = true;

const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
const auto hardware_info =
hardware_interface::parse_control_resources_from_urdf(urdf);

for (const auto & individual_hardware_info : hardware_info) {
std::string robot_hw_sim_type_str_ = individual_hardware_info.hardware_plugin_name;
RCLCPP_DEBUG(
logger_, "Load hardware interface %s ...",
robot_hw_sim_type_str_.c_str());
for (const auto &individual_hardware_info : hardware_info) {
std::string robot_hw_sim_type_str_ =
individual_hardware_info.hardware_plugin_name;
RCLCPP_DEBUG(logger_, "Load hardware interface %s ...",
robot_hw_sim_type_str_.c_str());

// Load hardware
std::unique_ptr<odri_gz_ros2_control::GazeboOdriSimSystemInterface> gzSimSystem;
std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
std::unique_ptr<odri_gz_ros2_control::GazeboOdriSimSystemInterface>
gzSimSystem;
std::scoped_lock guard(resource_interfaces_lock_,
claimed_command_interfaces_lock_);
try {
gzSimSystem = std::unique_ptr<odri_gz_ros2_control::GazeboOdriSimSystemInterface>(
gz_system_loader_.createUnmanagedInstance(robot_hw_sim_type_str_));
} catch (pluginlib::PluginlibException & ex) {
RCLCPP_ERROR(
logger_,
"The plugin failed to load for some reason. Error: %s\n",
ex.what());
gzSimSystem =
std::unique_ptr<odri_gz_ros2_control::GazeboOdriSimSystemInterface>(
gz_system_loader_.createUnmanagedInstance(
robot_hw_sim_type_str_));
} catch (pluginlib::PluginlibException &ex) {
RCLCPP_ERROR(logger_,
"The plugin failed to load for some reason. Error: %s\n",
ex.what());
continue;
}

// initialize simulation requirements
if (!gzSimSystem->initSim(
node_,
enabledJoints_,
individual_hardware_info,
*ecm_,
update_rate))
{
RCLCPP_FATAL(
logger_, "Could not initialize robot simulation interface");
if (!gzSimSystem->initSim(node_, enabledJoints_, individual_hardware_info,
*ecm_, update_rate)) {
RCLCPP_FATAL(logger_,
"Could not initialize robot simulation interface");
components_are_loaded_and_initialized_ = false;
break;
}
RCLCPP_DEBUG(
logger_, "Initialized robot simulation interface %s!",
robot_hw_sim_type_str_.c_str());
RCLCPP_DEBUG(logger_, "Initialized robot simulation interface %s!",
robot_hw_sim_type_str_.c_str());

// initialize hardware
import_component(std::move(gzSimSystem), individual_hardware_info);
Expand All @@ -124,13 +119,14 @@ class GZResourceManager : public hardware_interface::ResourceManager
return components_are_loaded_and_initialized_;
}

private:
private:
std::shared_ptr<rclcpp::Node> node_;
sim::EntityComponentManager * ecm_;
sim::EntityComponentManager *ecm_;
std::map<std::string, sim::Entity> enabledJoints_;

/// \brief Interface loader
pluginlib::ClassLoader<odri_gz_ros2_control::GazeboOdriSimSystemInterface> gz_system_loader_;
pluginlib::ClassLoader<odri_gz_ros2_control::GazeboOdriSimSystemInterface>
gz_system_loader_;

rclcpp::Logger logger_;
};
Expand Down Expand Up @@ -467,32 +463,34 @@ void GazeboOdriSimROS2ControlPlugin::Configure(
}

std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
std::make_unique<odri_gz_ros2_control::GZResourceManager>(this->dataPtr->node_, _ecm, enabledJoints);
std::make_unique<odri_gz_ros2_control::GZResourceManager>(
this->dataPtr->node_, _ecm, enabledJoints);

// Create the controller manager
RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager");
this->dataPtr->controller_manager_.reset(
new controller_manager::ControllerManager(
std::move(resource_manager_),
this->dataPtr->executor_,
controllerManagerNodeName,
this->dataPtr->node_->get_namespace()));
new controller_manager::ControllerManager(
std::move(resource_manager_), this->dataPtr->executor_,
controllerManagerNodeName, this->dataPtr->node_->get_namespace()));
this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_);

this->dataPtr->update_rate = this->dataPtr->controller_manager_->get_update_rate();
this->dataPtr->control_period_ = rclcpp::Duration(
std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / static_cast<double>(this->dataPtr->update_rate))));
this->dataPtr->update_rate =
this->dataPtr->controller_manager_->get_update_rate();
this->dataPtr->control_period_ =
rclcpp::Duration(std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(
1.0 / static_cast<double>(this->dataPtr->update_rate))));

// Force setting of use_sim_time parameter
this->dataPtr->controller_manager_->set_parameter(
rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true)));

// Wait for CM to receive robot description from the topic and then initialize Resource Manager
while (!this->dataPtr->controller_manager_->is_resource_manager_initialized()) {
RCLCPP_WARN(
this->dataPtr->node_->get_logger(),
"Waiting RM to load and initialize hardware...");
rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true)));

// Wait for CM to receive robot description from the topic and then initialize
// Resource Manager
while (
!this->dataPtr->controller_manager_->is_resource_manager_initialized()) {
RCLCPP_WARN(this->dataPtr->node_->get_logger(),
"Waiting RM to load and initialize hardware...");
std::this_thread::sleep_for(std::chrono::microseconds(2000000));
}

Expand All @@ -502,7 +500,6 @@ void GazeboOdriSimROS2ControlPlugin::Configure(
//////////////////////////////////////////////////
void GazeboOdriSimROS2ControlPlugin::PreUpdate(
const sim::UpdateInfo &_info, sim::EntityComponentManager & /*_ecm*/) {

if (!this->dataPtr->controller_manager_) {
return;
}
Expand Down
5 changes: 2 additions & 3 deletions src/gz_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -188,8 +188,7 @@ bool GazeboOdriSimSystem::initSim(
rclcpp::Node::SharedPtr &model_nh,
std::map<std::string, sim::Entity> &enableJoints,
const hardware_interface::HardwareInfo &hardware_info,
sim::EntityComponentManager &_ecm,
unsigned int update_rate) {
sim::EntityComponentManager &_ecm, unsigned int update_rate) {
this->dataPtr = std::make_unique<GazeboOdriSimSystemPrivate>();
this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time();

Expand Down Expand Up @@ -670,7 +669,7 @@ hardware_interface::return_type GazeboOdriSimSystem::write(
double error;
error = (this->dataPtr->joints_[i].joint_position -
this->dataPtr->joints_[i].joint_position_cmd) *
this->dataPtr->update_rate;
this->dataPtr->update_rate;

// Calculate target velcity
double target_vel = -this->dataPtr->position_proportional_gain_ * error;
Expand Down

0 comments on commit 9ecf73f

Please sign in to comment.