diff --git a/gz_ros2_control/include/gz_ros2_control/gz_system.hpp b/gz_ros2_control/include/gz_ros2_control/gz_system.hpp index 8ad075a2..c9ad741b 100644 --- a/gz_ros2_control/include/gz_ros2_control/gz_system.hpp +++ b/gz_ros2_control/include/gz_ros2_control/gz_system.hpp @@ -77,7 +77,7 @@ class GazeboSimSystem : public GazeboSimSystemInterface std::map & 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) diff --git a/gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp b/gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp index 60fa685f..dc6ebf5f 100644 --- a/gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp +++ b/gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp @@ -88,7 +88,7 @@ class GazeboSimSystemInterface std::map & 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_ diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index 9950e733..f42dcf8e 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -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 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 gzSimSystem; + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + try { + gzSimSystem = std::unique_ptr( + 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 node_; + sim::EntityComponentManager * ecm_; + std::map enabledJoints_; + + /// \brief Interface loader + pluginlib::ClassLoader 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 @@ -83,12 +163,6 @@ class GazeboSimROS2ControlPluginPrivate std::shared_ptr 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); @@ -165,61 +239,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( - 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 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()) @@ -268,22 +287,6 @@ void GazeboSimROS2ControlPlugin::Configure( } // Get params from SDF - std::string robot_param_node = _sdf->Get("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("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 arguments = {"--ros-args"}; auto sdfPtr = const_cast(_sdf.get()); @@ -322,10 +325,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 @@ -380,102 +379,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 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 resource_manager_ = - std::make_unique(); - - 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", - "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 gzSimSystem; - try { - gzSimSystem = std::unique_ptr( - 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 resource_manager_ = + std::make_unique(this->dataPtr->node_, _ecm, enabledJoints); // Create the controller manager RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager"); @@ -487,15 +412,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::duration(1.0 / static_cast(this->dataPtr->update_rate)))); @@ -504,6 +421,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; } diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 0a82f50e..2f48f954 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -140,7 +140,7 @@ class gz_ros2_control::GazeboSimSystemPrivate sim::EntityComponentManager * ecm; /// \brief controller update rate - int * update_rate; + unsigned int update_rate; /// \brief Gazebo communication node. GZ_TRANSPORT_NAMESPACE Node node; @@ -160,7 +160,7 @@ bool GazeboSimSystem::initSim( std::map & enableJoints, const hardware_interface::HardwareInfo & hardware_info, sim::EntityComponentManager & _ecm, - int & update_rate) + unsigned int update_rate) { this->dataPtr = std::make_unique(); this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); @@ -169,7 +169,7 @@ bool GazeboSimSystem::initSim( this->dataPtr->ecm = &_ecm; this->dataPtr->n_dof_ = hardware_info.joints.size(); - this->dataPtr->update_rate = &update_rate; + this->dataPtr->update_rate = update_rate; try { this->dataPtr->hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool(); @@ -462,10 +462,9 @@ void GazeboSimSystem::registerSensors( } CallbackReturn -GazeboSimSystem::on_init(const hardware_interface::HardwareInfo & actuator_info) +GazeboSimSystem::on_init(const hardware_interface::HardwareInfo & info) { - RCLCPP_WARN(this->nh_->get_logger(), "On init..."); - if (hardware_interface::SystemInterface::on_init(actuator_info) != CallbackReturn::SUCCESS) { + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; @@ -627,7 +626,7 @@ hardware_interface::return_type GazeboSimSystem::write( // Get error in position double error; error = (this->dataPtr->joints_[i].joint_position - - this->dataPtr->joints_[i].joint_position_cmd) * *this->dataPtr->update_rate; + this->dataPtr->joints_[i].joint_position_cmd) * this->dataPtr->update_rate; // Calculate target velcity double target_vel = -this->dataPtr->position_proportional_gain_ * error; @@ -690,7 +689,7 @@ hardware_interface::return_type GazeboSimSystem::write( double position_error = position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier; - double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate); + double velocity_sp = (-1.0) * position_error * this->dataPtr->update_rate; auto vel = this->dataPtr->ecm->Component(