Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Simplify access for robot description from CM by overriding RM. (backport #265) #364

Merged
merged 1 commit into from
Jul 8, 2024
Merged

Simplify access for robot description from CM by overriding RM. (backport #265) #364

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion gz_ros2_control/include/gz_ros2_control/gz_system.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading