Skip to content

Commit

Permalink
Simplify access for robot description from CM by overriding RM. (#265)
Browse files Browse the repository at this point in the history 
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
Co-authored-by: Sai Kishor Kothakota <[email protected]>
(cherry picked from commit ced470b)
  • Loading branch information
@destogl @mergify
destogl authored and mergify[bot] committed Jul 6, 2024
1 parent 8e19feb commit e77d45d
Show file tree
Hide file tree
Showing 4 changed files with 118 additions and 194 deletions.
2 changes: 1 addition & 1 deletion gz_ros2_control/include/gz_ros2_control/gz_system.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ class GazeboSimSystem : public GazeboSimSystemInterface
std::map<std::string, sim::Entity> & joints,
const hardware_interface::HardwareInfo & hardware_info,
sim::EntityComponentManager & _ecm,
int & update_rate) override;
unsigned int update_rate) override;

private:
// Register a sensor (for now just IMUs)
Expand Down
2 changes: 1 addition & 1 deletion gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ class GazeboSimSystemInterface
std::map<std::string, sim::Entity> & joints,
const hardware_interface::HardwareInfo & hardware_info,
sim::EntityComponentManager & _ecm,
int & update_rate) = 0;
unsigned int update_rate) = 0;

// Methods used to control a joint.
enum ControlMethod_
Expand Down
293 changes: 109 additions & 184 deletions gz_ros2_control/src/gz_ros2_control_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include <chrono>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <utility>
Expand Down Expand Up @@ -46,13 +47,92 @@

namespace 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(),
gz_system_loader_("gz_ros2_control", "gz_ros2_control::GazeboSimSystemInterface"),
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
{
components_are_loaded_and_initialized_ = true;

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());

// Load hardware
std::unique_ptr<gz_ros2_control::GazeboSimSystemInterface> gzSimSystem;
std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
try {
gzSimSystem = std::unique_ptr<gz_ros2_control::GazeboSimSystemInterface>(
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");
components_are_loaded_and_initialized_ = false;
break;
}
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);
}

return components_are_loaded_and_initialized_;
}

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

/// \brief Interface loader
pluginlib::ClassLoader<gz_ros2_control::GazeboSimSystemInterface> gz_system_loader_;

rclcpp::Logger logger_;
};

//////////////////////////////////////////////////
class GazeboSimROS2ControlPluginPrivate
{
public:
/// \brief Get the URDF XML from the parameter server
std::string getURDF() const;

/// \brief Get a list of enabled, unique, 1-axis joints of the model. If no
/// joint names are specified in the plugin configuration, all valid 1-axis
/// joints are returned
Expand Down Expand Up @@ -88,12 +168,6 @@ class GazeboSimROS2ControlPluginPrivate
std::shared_ptr<controller_manager::ControllerManager>
controller_manager_{nullptr};

/// \brief String with the robot description param_name
std::string robot_description_ = "robot_description";

/// \brief String with the name of the node that contains the robot_description
std::string robot_description_node_ = "robot_state_publisher";

/// \brief Last time the update method was called
rclcpp::Time last_update_sim_time_ros_ =
rclcpp::Time((int64_t)0, RCL_ROS_TIME);
Expand Down Expand Up @@ -170,61 +244,6 @@ GazeboSimROS2ControlPluginPrivate::GetEnabledJoints(
return output;
}

//////////////////////////////////////////////////
std::string GazeboSimROS2ControlPluginPrivate::getURDF() const
{
std::string urdf_string;

using namespace std::chrono_literals;
auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(
node_, robot_description_node_);
while (!parameters_client->wait_for_service(0.5s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(
node_->get_logger(), "Interrupted while waiting for %s service. Exiting.",
robot_description_node_.c_str());
return 0;
}
RCLCPP_ERROR(
node_->get_logger(), "%s service not available, waiting again...",
robot_description_node_.c_str());
}

RCLCPP_INFO(
node_->get_logger(), "connected to service!! %s asking for %s",
robot_description_node_.c_str(),
this->robot_description_.c_str());

// search and wait for robot_description on param server
while (urdf_string.empty()) {
RCLCPP_DEBUG(
node_->get_logger(), "param_name %s",
this->robot_description_.c_str());

try {
auto f = parameters_client->get_parameters({this->robot_description_});
f.wait();
std::vector<rclcpp::Parameter> values = f.get();
urdf_string = values[0].as_string();
} catch (const std::exception & e) {
RCLCPP_ERROR(node_->get_logger(), "%s", e.what());
}

if (!urdf_string.empty()) {
break;
} else {
RCLCPP_ERROR(
node_->get_logger(), "gz_ros2_control plugin is waiting for model"
" URDF in parameter [%s] on the ROS param server.",
this->robot_description_.c_str());
}
std::this_thread::sleep_for(std::chrono::microseconds(100000));
}
RCLCPP_INFO(node_->get_logger(), "Received URDF from param server");

return urdf_string;
}

//////////////////////////////////////////////////
GazeboSimROS2ControlPlugin::GazeboSimROS2ControlPlugin()
: dataPtr(std::make_unique<GazeboSimROS2ControlPluginPrivate>())
Expand Down Expand Up @@ -273,22 +292,6 @@ void GazeboSimROS2ControlPlugin::Configure(
}

// Get params from SDF
std::string robot_param_node = _sdf->Get<std::string>("robot_param_node");
if (!robot_param_node.empty()) {
this->dataPtr->robot_description_node_ = robot_param_node;
}
RCLCPP_INFO(
logger,
"robot_param_node is %s", this->dataPtr->robot_description_node_.c_str());

std::string robot_description = _sdf->Get<std::string>("robot_param");
if (!robot_description.empty()) {
this->dataPtr->robot_description_ = robot_description;
}
RCLCPP_INFO(
logger,
"robot_param_node is %s", this->dataPtr->robot_description_.c_str());

std::vector<std::string> arguments = {"--ros-args"};

auto sdfPtr = const_cast<sdf::Element *>(_sdf.get());
Expand Down Expand Up @@ -327,10 +330,6 @@ void GazeboSimROS2ControlPlugin::Configure(
if (ns.empty() || ns[0] != '/') {
ns = '/' + ns;
}

if (ns.length() > 1) {
this->dataPtr->robot_description_node_ = ns + "/" + this->dataPtr->robot_description_node_;
}
}

// Get list of remapping rules from SDF
Expand Down Expand Up @@ -385,102 +384,28 @@ void GazeboSimROS2ControlPlugin::Configure(
return;
}

// Read urdf from ros parameter server then
// setup actuators and mechanism control node.
// This call will block if ROS is not properly initialized.
std::string urdf_string;
std::vector<hardware_interface::HardwareInfo> control_hardware_info;
try {
urdf_string = this->dataPtr->getURDF();
control_hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf_string);
} catch (const std::runtime_error & ex) {
RCLCPP_ERROR_STREAM(
this->dataPtr->node_->get_logger(),
"Error parsing URDF in gz_ros2_control plugin, plugin not active : " << ex.what());
return;
}

std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
std::make_unique<hardware_interface::ResourceManager>();

try {
resource_manager_->load_urdf(urdf_string, false, false);
} catch (...) {
this->dataPtr->node_->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints));
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Error initializing URDF to resource manager!");
}
try {
this->dataPtr->robot_hw_sim_loader_.reset(
new pluginlib::ClassLoader<gz_ros2_control::GazeboSimSystemInterface>(
"gz_ros2_control",
"gz_ros2_control::GazeboSimSystemInterface"));
} catch (pluginlib::LibraryLoadException & ex) {
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has already been declared, %s",
e.what());
} catch (const rclcpp::exceptions::InvalidParametersException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Failed to create robot simulation interface loader: %s ",
ex.what());
return;
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has invalid name, %s",
e.what());
} catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value is invalid, %s",
e.what());
} catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s",
e.what());
}

for (unsigned int i = 0; i < control_hardware_info.size(); ++i) {
std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_plugin_name;
RCLCPP_DEBUG(
this->dataPtr->node_->get_logger(), "Load hardware interface %s ...",
robot_hw_sim_type_str_.c_str());

std::unique_ptr<gz_ros2_control::GazeboSimSystemInterface> gzSimSystem;
try {
gzSimSystem = std::unique_ptr<gz_ros2_control::GazeboSimSystemInterface>(
this->dataPtr->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_));
} catch (pluginlib::PluginlibException & ex) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(),
"The plugin failed to load for some reason. Error: %s\n",
ex.what());
continue;
}

try {
this->dataPtr->node_->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints));
} catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has already been declared, %s",
e.what());
} catch (const rclcpp::exceptions::InvalidParametersException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has invalid name, %s",
e.what());
} catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value is invalid, %s",
e.what());
} catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s",
e.what());
}

if (!gzSimSystem->initSim(
this->dataPtr->node_,
enabledJoints,
control_hardware_info[i],
_ecm,
this->dataPtr->update_rate))
{
RCLCPP_FATAL(
this->dataPtr->node_->get_logger(), "Could not initialize robot simulation interface");
return;
}
RCLCPP_DEBUG(
this->dataPtr->node_->get_logger(), "Initialized robot simulation interface %s!",
robot_hw_sim_type_str_.c_str());

resource_manager_->import_component(std::move(gzSimSystem), control_hardware_info[i]);

rclcpp_lifecycle::State state(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
hardware_interface::lifecycle_state_names::ACTIVE);
resource_manager_->set_component_state(control_hardware_info[i].name, state);
}
std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
std::make_unique<gz_ros2_control::GZResourceManager>(this->dataPtr->node_, _ecm, enabledJoints);

// Create the controller manager
RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager");
Expand All @@ -492,15 +417,7 @@ void GazeboSimROS2ControlPlugin::Configure(
this->dataPtr->node_->get_namespace()));
this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_);

if (!this->dataPtr->controller_manager_->has_parameter("update_rate")) {
RCLCPP_ERROR_STREAM(
this->dataPtr->node_->get_logger(),
"controller manager doesn't have an update_rate parameter");
return;
}

this->dataPtr->update_rate =
this->dataPtr->controller_manager_->get_parameter("update_rate").as_int();
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))));
Expand All @@ -509,6 +426,14 @@ void GazeboSimROS2ControlPlugin::Configure(
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...");
std::this_thread::sleep_for(std::chrono::microseconds(2000000));
}

this->dataPtr->entity_ = _entity;
}

Expand Down
Loading

0 comments on commit e77d45d

Please sign in to comment.