diff --git a/descriptions/unitree/go2_description/urdf/robot.urdf b/descriptions/unitree/go2_description/urdf/robot.urdf index 4c34838..dbdf128 100644 --- a/descriptions/unitree/go2_description/urdf/robot.urdf +++ b/descriptions/unitree/go2_description/urdf/robot.urdf @@ -194,7 +194,7 @@ - + @@ -247,7 +247,7 @@ - + @@ -275,7 +275,7 @@ - + @@ -303,7 +303,7 @@ - + @@ -387,7 +387,7 @@ - + @@ -415,7 +415,7 @@ - + @@ -443,7 +443,7 @@ - + @@ -527,7 +527,7 @@ - + @@ -555,7 +555,7 @@ - + @@ -583,7 +583,7 @@ - + @@ -667,7 +667,7 @@ - + @@ -695,7 +695,7 @@ - + @@ -723,7 +723,7 @@ - + diff --git a/hardwares/gz_quadruped_hardware/CMakeLists.txt b/hardwares/gz_quadruped_hardware/CMakeLists.txt index 0b24c23..ff63f3c 100644 --- a/hardwares/gz_quadruped_hardware/CMakeLists.txt +++ b/hardwares/gz_quadruped_hardware/CMakeLists.txt @@ -3,18 +3,18 @@ project(gz_quadruped_hardware) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # Default to C11 -if (NOT CMAKE_C_STANDARD) +if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 11) -endif () +endif() # Default to C++17 -if (NOT CMAKE_CXX_STANDARD) +if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) -endif () +endif() # Compiler options -if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) -endif () +endif() # Find dependencies find_package(ament_cmake REQUIRED) @@ -25,11 +25,28 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(yaml_cpp_vendor REQUIRED) -find_package(gz_sim_vendor REQUIRED) -find_package(gz-sim REQUIRED) +set(GZ_PLUGIN) +set(GZ_SIM) -find_package(gz_plugin_vendor REQUIRED) -find_package(gz-plugin REQUIRED) +find_package(gz-sim8 QUIET) +if(NOT gz-sim8_FOUND) + message(STATUS "Compiling against Gazebo fortress") + find_package(ignition-gazebo6 REQUIRED) + set(GZ_SIM_VER ${ignition-gazebo6_VERSION_MAJOR}) + find_package(ignition-plugin1 REQUIRED) + set(GZ_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) + set(GZ_PLUGIN ignition-plugin${GZ_PLUGIN_VER}::register) + set(GZ_SIM ignition-gazebo${GZ_SIM_VER}::core) +else () + message(STATUS "Compiling against Gazebo harmonic") + find_package(gz-sim8 REQUIRED) + set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) + find_package(gz-plugin2 REQUIRED) + set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) + set(GZ_PLUGIN gz-plugin${GZ_PLUGIN_VER}::register) + set(GZ_SIM gz-sim${GZ_SIM_VER}::core) + add_definitions(-DGZ_HEADERS) +endif() include_directories(include) @@ -38,8 +55,8 @@ add_library(${PROJECT_NAME}-system SHARED ) target_link_libraries(${PROJECT_NAME}-system - gz-sim::gz-sim - gz-plugin::register + ${GZ_SIM} + ${GZ_PLUGIN} ) ament_target_dependencies(${PROJECT_NAME}-system ament_index_cpp @@ -62,7 +79,7 @@ ament_target_dependencies(gz_quadruped_plugins rclcpp ) target_link_libraries(gz_quadruped_plugins - gz-sim::gz-sim + ${GZ_SIM} ) ## Install diff --git a/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_quadruped_plugin.hpp b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_quadruped_plugin.hpp index 92d90e1..1af622e 100644 --- a/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_quadruped_plugin.hpp +++ b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_quadruped_plugin.hpp @@ -15,18 +15,25 @@ #include +#ifdef GZ_HEADERS #include namespace sim = gz::sim; +#else +#include +namespace sim = ignition::gazebo; +#endif -namespace gz_quadruped_hardware { +namespace gz_quadruped_hardware +{ // Forward declarations. class GazeboSimQuadrupedPluginPrivate; class GazeboSimQuadrupedPlugin - : public sim::System, - public sim::ISystemConfigure, - public sim::ISystemPreUpdate, - public sim::ISystemPostUpdate { + : public sim::System, + public sim::ISystemConfigure, + public sim::ISystemPreUpdate, + public sim::ISystemPostUpdate + { public: /// \brief Constructor GazeboSimQuadrupedPlugin(); @@ -36,22 +43,22 @@ namespace gz_quadruped_hardware { // Documentation inherited void Configure( - const sim::Entity &_entity, - const std::shared_ptr &_sdf, - sim::EntityComponentManager &_ecm, - sim::EventManager &_eventMgr) override; + const sim::Entity& _entity, + const std::shared_ptr& _sdf, + sim::EntityComponentManager& _ecm, + sim::EventManager& _eventMgr) override; // Documentation inherited void PreUpdate( - const sim::UpdateInfo &_info, - sim::EntityComponentManager &_ecm) override; + const sim::UpdateInfo& _info, + sim::EntityComponentManager& _ecm) override; void PostUpdate( - const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) override; + const sim::UpdateInfo& _info, + const sim::EntityComponentManager& _ecm) override; private: /// \brief Private data pointer. std::unique_ptr dataPtr; }; -} // namespace gz_ros2_control +} diff --git a/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system.hpp b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system.hpp index f376201..e252275 100644 --- a/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system.hpp +++ b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system.hpp @@ -13,7 +13,6 @@ // limitations under the License. #pragma once - #include #include #include @@ -23,7 +22,8 @@ #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -namespace gz_quadruped_hardware { +namespace gz_quadruped_hardware +{ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; // Forward declaration @@ -32,13 +32,14 @@ namespace gz_quadruped_hardware { // These class must inherit `gz_ros2_control::GazeboSimSystemInterface` which implements a // simulated `ros2_control` `hardware_interface::SystemInterface`. - class GazeboSimSystem final : public GazeboSimSystemInterface { + class GazeboSimSystem : public GazeboSimSystemInterface + { public: // Documentation Inherited - CallbackReturn on_init(const hardware_interface::HardwareInfo &system_info) + CallbackReturn on_init(const hardware_interface::HardwareInfo& system_info) override; - CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; // Documentation Inherited std::vector export_state_interfaces() override; @@ -47,37 +48,42 @@ namespace gz_quadruped_hardware { std::vector export_command_interfaces() override; // Documentation Inherited - CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; // Documentation Inherited - CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + // Documentation Inherited + hardware_interface::return_type perform_command_mode_switch( + const std::vector& start_interfaces, + const std::vector& stop_interfaces) override; // Documentation Inherited hardware_interface::return_type read( - const rclcpp::Time &time, - const rclcpp::Duration &period) override; + const rclcpp::Time& time, + const rclcpp::Duration& period) override; // Documentation Inherited hardware_interface::return_type write( - const rclcpp::Time &time, - const rclcpp::Duration &period) override; + const rclcpp::Time& time, + const rclcpp::Duration& period) override; // Documentation Inherited bool initSim( - rclcpp::Node::SharedPtr &model_nh, - std::map &joints, - const hardware_interface::HardwareInfo &hardware_info, - sim::EntityComponentManager &_ecm, - unsigned int update_rate) override; + rclcpp::Node::SharedPtr& model_nh, + std::map& joints, + const hardware_interface::HardwareInfo& hardware_info, + sim::EntityComponentManager& _ecm, + int& update_rate) override; private: // Register a sensor (for now just IMUs) // \param[in] hardware_info hardware information where the data of // the sensors is extract. void registerSensors( - const hardware_interface::HardwareInfo &hardware_info); + const hardware_interface::HardwareInfo& hardware_info); /// \brief Private data class std::unique_ptr dataPtr; }; -} // namespace gz_ros2_control +} diff --git a/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system_interface.hpp b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system_interface.hpp index e2955b5..9d53226 100644 --- a/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system_interface.hpp +++ b/hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system_interface.hpp @@ -16,15 +16,23 @@ #include #include #include +#include +#ifdef GZ_HEADERS #include namespace sim = gz::sim; +#else +#include +namespace sim = ignition::gazebo; +#endif #include +#include #include -namespace gz_quadruped_hardware { +namespace gz_quadruped_hardware +{ /// \brief This class allows us to handle flags easily, instead of using strings /// /// For example @@ -42,44 +50,53 @@ namespace gz_quadruped_hardware { /// foo & POSITION -> True // Check if position is active in the flag /// foo & VELOCITY -> False // Check if velocity is active in the flag - template::type> - class SafeEnum { + template ::type> + class SafeEnum + { public: SafeEnum() - : mFlags(0) { + : mFlags(0) + { } explicit SafeEnum(ENUM singleFlag) - : mFlags(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; } - SafeEnum operator|(ENUM addValue) { + SafeEnum operator|(ENUM addValue) + { SafeEnum result(*this); result |= addValue; return result; } - SafeEnum &operator&=(ENUM maskValue) { + SafeEnum& operator&=(ENUM maskValue) + { mFlags &= maskValue; return *this; } - SafeEnum operator&(ENUM maskValue) { + SafeEnum operator&(ENUM maskValue) + { SafeEnum result(*this); result &= maskValue; return result; } - SafeEnum operator~() { + SafeEnum operator~() + { SafeEnum result(*this); result.mFlags = ~result.mFlags; return result; @@ -93,7 +110,8 @@ namespace gz_quadruped_hardware { // SystemInterface provides API-level access to read and command joint properties. class GazeboSimSystemInterface - : public hardware_interface::SystemInterface { + : public hardware_interface::SystemInterface + { public: /// \brief Initialize the system interface /// param[in] model_nh Pointer to the ros2 node @@ -103,23 +121,24 @@ namespace gz_quadruped_hardware { /// param[in] _ecm Entity-component manager. /// param[in] update_rate controller update rate virtual bool initSim( - rclcpp::Node::SharedPtr &model_nh, - std::map &joints, - const hardware_interface::HardwareInfo &hardware_info, - sim::EntityComponentManager &_ecm, - unsigned int update_rate) = 0; + rclcpp::Node::SharedPtr& model_nh, + std::map& joints, + const hardware_interface::HardwareInfo& hardware_info, + sim::EntityComponentManager& _ecm, + int& update_rate) = 0; // Methods used to control a joint. - enum ControlMethod_ { + enum ControlMethod_ + { NONE = 0, POSITION = (1 << 0), VELOCITY = (1 << 1), EFFORT = (1 << 2), }; - typedef SafeEnum ControlMethod; + typedef SafeEnum ControlMethod; protected: rclcpp::Node::SharedPtr nh_; }; -} // namespace gz_ros2_control +} diff --git a/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp b/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp index 68fae82..1cbc43b 100644 --- a/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp +++ b/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp @@ -17,12 +17,13 @@ #include #include #include -#include +#include #include #include #include #include +#ifdef GZ_HEADERS #include #include #include @@ -30,12 +31,21 @@ #include #include #include - +#else +#include +#include +#include +#include +#include +#include +#include +#endif #include #include #include +#include #include @@ -44,89 +54,15 @@ #include "gz_quadruped_hardware/gz_quadruped_plugin.hpp" #include "gz_quadruped_hardware/gz_system.hpp" -namespace gz_quadruped_hardware { - class GZResourceManager : public hardware_interface::ResourceManager { - public: - GZResourceManager( - rclcpp::Node::SharedPtr &node, - sim::EntityComponentManager &ecm, - std::map enabledJoints) - : ResourceManager( - node->get_node_clock_interface(), node->get_node_logging_interface()), - gz_system_loader_("gz_quadruped_hardware", "gz_quadruped_hardware::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_; - }; - +namespace gz_quadruped_hardware +{ ////////////////////////////////////////////////// - class GazeboSimQuadrupedPluginPrivate { + class GazeboSimQuadrupedPluginPrivate + { 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 @@ -135,8 +71,8 @@ namespace gz_quadruped_hardware { /// \param[in] _ecm Gazebo Entity Component Manager /// \return List of entities containing all enabled joints std::map GetEnabledJoints( - const sim::Entity &_entity, - sim::EntityComponentManager &_ecm) const; + const sim::Entity& _entity, + sim::EntityComponentManager& _ecm) const; /// \brief Entity ID for sensor within Gazebo. sim::Entity entity_; @@ -153,16 +89,27 @@ namespace gz_quadruped_hardware { /// \brief Timing rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); + /// \brief Interface loader + std::shared_ptr> + robot_hw_sim_loader_{nullptr}; + /// \brief Controller manager 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(static_cast(0), RCL_ROS_TIME); + rclcpp::Time((int64_t)0, RCL_ROS_TIME); /// \brief ECM pointer - sim::EntityComponentManager *ecm{nullptr}; + sim::EntityComponentManager* ecm{nullptr}; /// \brief controller update rate int update_rate; @@ -171,8 +118,9 @@ namespace gz_quadruped_hardware { ////////////////////////////////////////////////// std::map GazeboSimQuadrupedPluginPrivate::GetEnabledJoints( - const sim::Entity &_entity, - sim::EntityComponentManager &_ecm) const { + const sim::Entity& _entity, + sim::EntityComponentManager& _ecm) const + { std::map output; std::vector enabledJoints; @@ -181,32 +129,37 @@ namespace gz_quadruped_hardware { auto jointEntities = _ecm.ChildrenByComponents(_entity, sim::components::Joint()); // Iterate over all joints and verify whether they can be enabled or not - for (const auto &jointEntity: jointEntities) { + for (const auto& jointEntity : jointEntities) + { const auto jointName = _ecm.Component( jointEntity)->Data(); // Make sure the joint type is supported, i.e. it has exactly one // actuated axis - const auto *jointType = _ecm.Component(jointEntity); - switch (jointType->Data()) { - case sdf::JointType::PRISMATIC: - case sdf::JointType::REVOLUTE: - case sdf::JointType::CONTINUOUS: - case sdf::JointType::GEARBOX: { + const auto* jointType = _ecm.Component(jointEntity); + switch (jointType->Data()) + { + case sdf::JointType::PRISMATIC: + case sdf::JointType::REVOLUTE: + case sdf::JointType::CONTINUOUS: + case sdf::JointType::GEARBOX: + { // Supported joint type break; } - case sdf::JointType::FIXED: { + case sdf::JointType::FIXED: + { RCLCPP_INFO( node_->get_logger(), "[gz_quadruped_hardware] Fixed joint [%s] (Entity=%lu)] is skipped", jointName.c_str(), jointEntity); continue; } - case sdf::JointType::REVOLUTE2: - case sdf::JointType::SCREW: - case sdf::JointType::BALL: - case sdf::JointType::UNIVERSAL: { + case sdf::JointType::REVOLUTE2: + case sdf::JointType::SCREW: + case sdf::JointType::BALL: + case sdf::JointType::UNIVERSAL: + { RCLCPP_WARN( node_->get_logger(), "[gz_quadruped_hardware] Joint [%s] (Entity=%lu)] is of unsupported type." @@ -214,7 +167,8 @@ namespace gz_quadruped_hardware { jointName.c_str(), jointEntity); continue; } - default: { + default: + { RCLCPP_WARN( node_->get_logger(), "[gz_quadruped_hardware] Joint [%s] (Entity=%lu)] is of unknown type", @@ -229,14 +183,81 @@ namespace gz_quadruped_hardware { } ////////////////////////////////////////////////// - GazeboSimQuadrupedPlugin::GazeboSimQuadrupedPlugin() - : dataPtr(std::make_unique()) { + std::string GazeboSimQuadrupedPluginPrivate::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_quadruped_hardware 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; } ////////////////////////////////////////////////// - GazeboSimQuadrupedPlugin::~GazeboSimQuadrupedPlugin() { + GazeboSimQuadrupedPlugin::GazeboSimQuadrupedPlugin() + : dataPtr(std::make_unique()) + { + } + + ////////////////////////////////////////////////// + GazeboSimQuadrupedPlugin::~GazeboSimQuadrupedPlugin() + { // Stop controller manager thread - if (!this->dataPtr->controller_manager_) { + if (!this->dataPtr->controller_manager_) + { return; } this->dataPtr->executor_->remove_node(this->dataPtr->controller_manager_); @@ -246,39 +267,61 @@ namespace gz_quadruped_hardware { ////////////////////////////////////////////////// void GazeboSimQuadrupedPlugin::Configure( - const sim::Entity &_entity, - const std::shared_ptr &_sdf, - sim::EntityComponentManager &_ecm, - sim::EventManager &) { - rclcpp::Logger logger = rclcpp::get_logger("GazeboSimROS2ControlPlugin"); + const sim::Entity& _entity, + const std::shared_ptr& _sdf, + sim::EntityComponentManager& _ecm, + sim::EventManager&) + { + rclcpp::Logger logger = rclcpp::get_logger("GazeboSimQuadrupedPlugin"); // Make sure the controller is attached to a valid model const auto model = sim::Model(_entity); - if (!model.Valid(_ecm)) { + if (!model.Valid(_ecm)) + { RCLCPP_ERROR( logger, - "[Gazebo ROS 2 Control] Failed to initialize because [%s] (Entity=%lu)] is not a model." - "Please make sure that Gazebo ROS 2 Control is attached to a valid model.", + "[gz_quadruped_hardware] Failed to initialize because [%s] (Entity=%lu)] is not a model." + "Please make sure that gz_quadruped_hardware is attached to a valid model.", model.Name(_ecm).c_str(), _entity); return; } // Get params from SDF - auto param_file_name = _sdf->Get("parameters"); + std::string paramFileName = _sdf->Get("parameters"); - if (param_file_name.empty()) { + if (paramFileName.empty()) + { RCLCPP_ERROR( logger, - "Gazebo quadruped ros2 control found an empty parameters file. Failed to initialize."); + "gz_quadruped_hardware found an empty parameters file. Failed to initialize."); return; } // 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()); + auto sdfPtr = const_cast(_sdf.get()); sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters"); - while (argument_sdf) { + while (argument_sdf) + { std::string argument = argument_sdf->Get(); arguments.push_back(RCL_PARAM_FILE_FLAG); arguments.push_back(argument); @@ -288,30 +331,39 @@ namespace gz_quadruped_hardware { // Get controller manager node name std::string controllerManagerNodeName{"controller_manager"}; - if (sdfPtr->HasElement("controller_manager_name")) { + if (sdfPtr->HasElement("controller_manager_name")) + { controllerManagerNodeName = sdfPtr->GetElement("controller_manager_name")->Get(); } std::string ns = "/"; - - if (sdfPtr->HasElement("ros")) { + if (sdfPtr->HasElement("ros")) + { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); // Set namespace if tag is present - if (sdfRos->HasElement("namespace")) { + if (sdfRos->HasElement("namespace")) + { ns = sdfRos->GetElement("namespace")->Get(); // prevent exception: namespace must be absolute, it must lead with a '/' - if (ns.empty() || ns[0] != '/') { + 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 - if (sdfRos->HasElement("remapping")) { + if (sdfRos->HasElement("remapping")) + { sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); arguments.push_back(RCL_ROS_ARGS_FLAG); - while (argument_sdf) { + while (argument_sdf) + { auto argument = argument_sdf->Get(); arguments.push_back(RCL_REMAP_FLAG); arguments.push_back(argument); @@ -320,19 +372,15 @@ namespace gz_quadruped_hardware { } } - std::vector argv; - for (const auto &arg: arguments) { - argv.push_back(arg.data()); - } // Create a default context, if not already - if (!rclcpp::ok()) { - init( - static_cast(argv.size()), argv.data(), rclcpp::InitOptions(), - rclcpp::SignalHandlerOptions::None); + if (!rclcpp::ok()) + { + RCLCPP_DEBUG_STREAM(logger, "Create default context"); + std::vector argv; + rclcpp::init(static_cast(argv.size()), argv.data()); } - std::string node_name = "gz_quadruped_control"; - + std::string node_name = "gz_quadruped_hardware"; this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns); this->dataPtr->executor_ = std::make_shared(); this->dataPtr->executor_->add_node(this->dataPtr->node_); @@ -342,8 +390,54 @@ namespace gz_quadruped_hardware { }; this->dataPtr->thread_executor_spin_ = std::thread(spin); + RCLCPP_DEBUG_STREAM(logger, "Create node " << node_name); + + // Read urdf from ros parameter server + const auto urdf_string = this->dataPtr->getURDF(); + if (urdf_string.empty()) + { + RCLCPP_ERROR_STREAM(this->dataPtr->node_->get_logger(), "An empty URDF was passed. Exiting."); + return; + } + + // set the robot description as argument to propagate it among controller manager and controllers + // Remove all comments via regex pattern to match XML comments, including newlines + const std::regex comment_pattern(R"()"); + const auto rb_arg = std::string("robot_description:=") + std::regex_replace( + urdf_string, + comment_pattern, ""); + arguments.push_back(RCL_PARAM_FLAG); + arguments.push_back(rb_arg); + + std::vector argv; + for (const auto& arg : arguments) + { + argv.push_back(reinterpret_cast(arg.data())); + } + + // set the arguments into rcl context + rcl_arguments_t rcl_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t rcl_ret = rcl_parse_arguments( + static_cast(argv.size()), + argv.data(), rcl_get_default_allocator(), &rcl_args); + auto rcl_context = + this->dataPtr->node_->get_node_base_interface()->get_context()->get_rcl_context(); + rcl_context->global_arguments = rcl_args; + if (rcl_ret != RCL_RET_OK) + { + RCLCPP_ERROR_STREAM( + this->dataPtr->node_->get_logger(), "Argument parser error: " << rcl_get_error_string().str); + rcl_reset_error(); + return; + } + if (rcl_arguments_get_param_files_count(&rcl_args) < 1) + { + RCLCPP_ERROR(this->dataPtr->node_->get_logger(), "Failed to parse input yaml file(s)"); + return; + } + RCLCPP_DEBUG_STREAM( - this->dataPtr->node_->get_logger(), "[Gazebo Quadruped ROS2 Control] Setting up controller for [" << + this->dataPtr->node_->get_logger(), "[gz_quadruped_hardware] Setting up controller for [" << model.Name(_ecm) << "] (Entity=" << _entity << ")]."); // Get list of enabled joints @@ -351,15 +445,100 @@ namespace gz_quadruped_hardware { _entity, _ecm); - if (enabledJoints.size() == 0) { + if (enabledJoints.size() == 0) + { RCLCPP_DEBUG_STREAM( this->dataPtr->node_->get_logger(), - "[Gazebo Quadruped ROS2 Control] There are no available Joints."); + "[gz_quadruped_hardware] There are no available Joints."); + 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::vector control_hardware_info; + try + { + 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_quadruped_hardware plugin, plugin not active : " << ex.what()); return; } std::unique_ptr resource_manager_ = - std::make_unique(this->dataPtr->node_, _ecm, enabledJoints); + std::make_unique(); + + try + { + resource_manager_->load_urdf(urdf_string, false, false); + } + catch (...) + { + 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_quadruped_hardware", + "gz_quadruped_hardware::GazeboSimSystemInterface")); + } + catch (pluginlib::LibraryLoadException& ex) + { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), "Failed to create robot simulation interface loader: %s ", + ex.what()); + return; + } + + for (unsigned int i = 0; i < control_hardware_info.size(); ++i) + { + std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_class_type; + std::unique_ptr gzSimSystem; + RCLCPP_DEBUG( + this->dataPtr->node_->get_logger(), "Load hardware interface %s ...", + robot_hw_sim_type_str_.c_str()); + + 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; + } + 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); + } // Create the controller manager RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager"); @@ -374,10 +553,19 @@ namespace gz_quadruped_hardware { std::move(resource_manager_), this->dataPtr->executor_, controllerManagerNodeName, - this->dataPtr->node_->get_namespace(), options)); + 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(); + 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->control_period_ = rclcpp::Duration( std::chrono::duration_cast( std::chrono::duration(1.0 / static_cast(this->dataPtr->update_rate)))); @@ -386,36 +574,34 @@ namespace gz_quadruped_hardware { 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; } ////////////////////////////////////////////////// void GazeboSimQuadrupedPlugin::PreUpdate( - const sim::UpdateInfo &_info, - sim::EntityComponentManager & /*_ecm*/) { - if (!this->dataPtr->controller_manager_) { + const sim::UpdateInfo& _info, + sim::EntityComponentManager& /*_ecm*/) + { + if (!this->dataPtr->controller_manager_) + { return; } static bool warned{false}; - if (!warned) { + if (!warned) + { rclcpp::Duration gazebo_period(_info.dt); // Check the period against the simulation period - if (this->dataPtr->control_period_ < _info.dt) { + if (this->dataPtr->control_period_ < _info.dt) + { RCLCPP_ERROR_STREAM( this->dataPtr->node_->get_logger(), "Desired controller update period (" << this->dataPtr->control_period_.seconds() << " s) is faster than the gazebo simulation period (" << gazebo_period.seconds() << " s)."); - } else if (this->dataPtr->control_period_ > gazebo_period) { + } + else if (this->dataPtr->control_period_ > gazebo_period) + { RCLCPP_WARN_STREAM( this->dataPtr->node_->get_logger(), " Desired controller update period (" << this->dataPtr->control_period_.seconds() << @@ -425,9 +611,9 @@ namespace gz_quadruped_hardware { warned = true; } - const rclcpp::Time sim_time_ros(std::chrono::duration_cast( - _info.simTime).count(), RCL_ROS_TIME); - const rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; + rclcpp::Time sim_time_ros(std::chrono::duration_cast( + _info.simTime).count(), RCL_ROS_TIME); + rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; // Always set commands on joints, otherwise at low control frequencies the joints tremble // as they are updated at a fraction of gazebo sim time this->dataPtr->controller_manager_->write(sim_time_ros, sim_period); @@ -435,30 +621,42 @@ namespace gz_quadruped_hardware { ////////////////////////////////////////////////// void GazeboSimQuadrupedPlugin::PostUpdate( - const sim::UpdateInfo &_info, - const sim::EntityComponentManager & /*_ecm*/) { - if (!this->dataPtr->controller_manager_) { + const sim::UpdateInfo& _info, + const sim::EntityComponentManager& /*_ecm*/) + { + if (!this->dataPtr->controller_manager_) + { return; } // Get the simulation time and period - const rclcpp::Time sim_time_ros(std::chrono::duration_cast( - _info.simTime).count(), RCL_ROS_TIME); - const rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; + rclcpp::Time sim_time_ros(std::chrono::duration_cast( + _info.simTime).count(), RCL_ROS_TIME); + rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; - if (sim_period >= this->dataPtr->control_period_) { + if (sim_period >= this->dataPtr->control_period_) + { this->dataPtr->last_update_sim_time_ros_ = sim_time_ros; auto gz_controller_manager = - std::dynamic_pointer_cast( - this->dataPtr->controller_manager_); + std::dynamic_pointer_cast( + this->dataPtr->controller_manager_); this->dataPtr->controller_manager_->read(sim_time_ros, sim_period); this->dataPtr->controller_manager_->update(sim_time_ros, sim_period); } } } // namespace gz_quadruped_hardware +#ifdef GZ_HEADERS GZ_ADD_PLUGIN( gz_quadruped_hardware::GazeboSimQuadrupedPlugin, - gz::sim::System, + sim::System, gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemConfigure, gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPreUpdate, gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPostUpdate) +#else +IGNITION_ADD_PLUGIN( + gz_quadruped_hardware::GazeboSimQuadrupedPlugin, + sim::System, + gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemConfigure, + gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPreUpdate, + gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPostUpdate) +#endif diff --git a/hardwares/gz_quadruped_hardware/src/gz_ros2_control_plugin.cpp b/hardwares/gz_quadruped_hardware/src/gz_ros2_control_plugin.cpp new file mode 100644 index 0000000..d4c51e2 --- /dev/null +++ b/hardwares/gz_quadruped_hardware/src/gz_ros2_control_plugin.cpp @@ -0,0 +1,611 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef GZ_HEADERS +#include +#include +#include +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#include +#include +#endif + +#include + +#include +#include +#include + +#include + +#include + +#include "gz_ros2_control/gz_ros2_control_plugin.hpp" +#include "gz_ros2_control/gz_system.hpp" + +namespace gz_quadruped_hardware +{ +////////////////////////////////////////////////// +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 + /// \param[in] _entity Entity of the model that the plugin is being + /// configured for + /// \param[in] _ecm Gazebo Entity Component Manager + /// \return List of entities containing all enabled joints + std::map GetEnabledJoints( + const sim::Entity & _entity, + sim::EntityComponentManager & _ecm) const; + + /// \brief Entity ID for sensor within Gazebo. + sim::Entity entity_; + + /// \brief Node Handles + std::shared_ptr node_{nullptr}; + + /// \brief Thread where the executor will spin + std::thread thread_executor_spin_; + + /// \brief Executor to spin the controller + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + + /// \brief Timing + rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); + + /// \brief Interface loader + std::shared_ptr> + robot_hw_sim_loader_{nullptr}; + + /// \brief Controller manager + 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); + + /// \brief ECM pointer + sim::EntityComponentManager * ecm{nullptr}; + + /// \brief controller update rate + int update_rate; +}; + +////////////////////////////////////////////////// +std::map +GazeboSimROS2ControlPluginPrivate::GetEnabledJoints( + const sim::Entity & _entity, + sim::EntityComponentManager & _ecm) const +{ + std::map output; + + std::vector enabledJoints; + + // Get all available joints + auto jointEntities = _ecm.ChildrenByComponents(_entity, sim::components::Joint()); + + // Iterate over all joints and verify whether they can be enabled or not + for (const auto & jointEntity : jointEntities) { + const auto jointName = _ecm.Component( + jointEntity)->Data(); + + // Make sure the joint type is supported, i.e. it has exactly one + // actuated axis + const auto * jointType = _ecm.Component(jointEntity); + switch (jointType->Data()) { + case sdf::JointType::PRISMATIC: + case sdf::JointType::REVOLUTE: + case sdf::JointType::CONTINUOUS: + case sdf::JointType::GEARBOX: + { + // Supported joint type + break; + } + case sdf::JointType::FIXED: + { + RCLCPP_INFO( + node_->get_logger(), + "[gz_ros2_control] Fixed joint [%s] (Entity=%lu)] is skipped", + jointName.c_str(), jointEntity); + continue; + } + case sdf::JointType::REVOLUTE2: + case sdf::JointType::SCREW: + case sdf::JointType::BALL: + case sdf::JointType::UNIVERSAL: + { + RCLCPP_WARN( + node_->get_logger(), + "[gz_ros2_control] Joint [%s] (Entity=%lu)] is of unsupported type." + " Only joints with a single axis are supported.", + jointName.c_str(), jointEntity); + continue; + } + default: + { + RCLCPP_WARN( + node_->get_logger(), + "[gz_ros2_control] Joint [%s] (Entity=%lu)] is of unknown type", + jointName.c_str(), jointEntity); + continue; + } + } + output[jointName] = jointEntity; + } + + 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()) +{ +} + +////////////////////////////////////////////////// +GazeboSimROS2ControlPlugin::~GazeboSimROS2ControlPlugin() +{ + // Stop controller manager thread + if (!this->dataPtr->controller_manager_) { + return; + } + this->dataPtr->executor_->remove_node(this->dataPtr->controller_manager_); + this->dataPtr->executor_->cancel(); + this->dataPtr->thread_executor_spin_.join(); +} + +////////////////////////////////////////////////// +void GazeboSimROS2ControlPlugin::Configure( + const sim::Entity & _entity, + const std::shared_ptr & _sdf, + sim::EntityComponentManager & _ecm, + sim::EventManager &) +{ + rclcpp::Logger logger = rclcpp::get_logger("GazeboSimROS2ControlPlugin"); + // Make sure the controller is attached to a valid model + const auto model = sim::Model(_entity); + if (!model.Valid(_ecm)) { + RCLCPP_ERROR( + logger, + "[gz_ros2_control] Failed to initialize because [%s] (Entity=%lu)] is not a model." + "Please make sure that gz_ros2_control is attached to a valid model.", + model.Name(_ecm).c_str(), _entity); + return; + } + + // Get params from SDF + std::string paramFileName = _sdf->Get("parameters"); + + if (paramFileName.empty()) { + RCLCPP_ERROR( + logger, + "gz_ros2_control found an empty parameters file. Failed to initialize."); + return; + } + + // 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()); + + sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters"); + while (argument_sdf) { + std::string argument = argument_sdf->Get(); + arguments.push_back(RCL_PARAM_FILE_FLAG); + arguments.push_back(argument); + argument_sdf = argument_sdf->GetNextElement("parameters"); + } + + // Get controller manager node name + std::string controllerManagerNodeName{"controller_manager"}; + + if (sdfPtr->HasElement("controller_manager_name")) { + controllerManagerNodeName = sdfPtr->GetElement("controller_manager_name")->Get(); + } + + std::string ns = "/"; + if (sdfPtr->HasElement("ros")) { + sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); + + // Set namespace if tag is present + if (sdfRos->HasElement("namespace")) { + ns = sdfRos->GetElement("namespace")->Get(); + // prevent exception: namespace must be absolute, it must lead with a '/' + 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 + if (sdfRos->HasElement("remapping")) { + sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); + + arguments.push_back(RCL_ROS_ARGS_FLAG); + while (argument_sdf) { + std::string argument = argument_sdf->Get(); + arguments.push_back(RCL_REMAP_FLAG); + arguments.push_back(argument); + argument_sdf = argument_sdf->GetNextElement("remapping"); + } + } + } + + // Create a default context, if not already + if (!rclcpp::ok()) { + RCLCPP_DEBUG_STREAM(logger, "Create default context"); + std::vector argv; + rclcpp::init(static_cast(argv.size()), argv.data()); + } + + std::string node_name = "gz_ros2_control"; + this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns); + this->dataPtr->executor_ = std::make_shared(); + this->dataPtr->executor_->add_node(this->dataPtr->node_); + auto spin = [this]() + { + this->dataPtr->executor_->spin(); + }; + this->dataPtr->thread_executor_spin_ = std::thread(spin); + + RCLCPP_DEBUG_STREAM(logger, "Create node " << node_name); + + // Read urdf from ros parameter server + const auto urdf_string = this->dataPtr->getURDF(); + if (urdf_string.empty()) { + RCLCPP_ERROR_STREAM(this->dataPtr->node_->get_logger(), "An empty URDF was passed. Exiting."); + return; + } + + // set the robot description as argument to propagate it among controller manager and controllers + // Remove all comments via regex pattern to match XML comments, including newlines + const std::regex comment_pattern(R"()"); + const auto rb_arg = std::string("robot_description:=") + std::regex_replace( + urdf_string, + comment_pattern, ""); + arguments.push_back(RCL_PARAM_FLAG); + arguments.push_back(rb_arg); + + std::vector argv; + for (const auto & arg : arguments) { + argv.push_back(reinterpret_cast(arg.data())); + } + + // set the arguments into rcl context + rcl_arguments_t rcl_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t rcl_ret = rcl_parse_arguments( + static_cast(argv.size()), + argv.data(), rcl_get_default_allocator(), &rcl_args); + auto rcl_context = + this->dataPtr->node_->get_node_base_interface()->get_context()->get_rcl_context(); + rcl_context->global_arguments = rcl_args; + if (rcl_ret != RCL_RET_OK) { + RCLCPP_ERROR_STREAM( + this->dataPtr->node_->get_logger(), "Argument parser error: " << rcl_get_error_string().str); + rcl_reset_error(); + return; + } + if (rcl_arguments_get_param_files_count(&rcl_args) < 1) { + RCLCPP_ERROR(this->dataPtr->node_->get_logger(), "Failed to parse input yaml file(s)"); + return; + } + + RCLCPP_DEBUG_STREAM( + this->dataPtr->node_->get_logger(), "[gz_ros2_control] Setting up controller for [" << + model.Name(_ecm) << "] (Entity=" << _entity << ")]."); + + // Get list of enabled joints + auto enabledJoints = this->dataPtr->GetEnabledJoints( + _entity, + _ecm); + + if (enabledJoints.size() == 0) { + RCLCPP_DEBUG_STREAM( + this->dataPtr->node_->get_logger(), + "[gz_ros2_control] There are no available Joints."); + 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::vector control_hardware_info; + try { + 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 (...) { + 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) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), "Failed to create robot simulation interface loader: %s ", + ex.what()); + return; + } + + for (unsigned int i = 0; i < control_hardware_info.size(); ++i) { + std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_class_type; + std::unique_ptr gzSimSystem; + RCLCPP_DEBUG( + this->dataPtr->node_->get_logger(), "Load hardware interface %s ...", + robot_hw_sim_type_str_.c_str()); + + 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; + } + 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); + } + + // 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())); + 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->control_period_ = rclcpp::Duration( + std::chrono::duration_cast( + std::chrono::duration(1.0 / static_cast(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))); + + this->dataPtr->entity_ = _entity; +} + +////////////////////////////////////////////////// +void GazeboSimROS2ControlPlugin::PreUpdate( + const sim::UpdateInfo & _info, + sim::EntityComponentManager & /*_ecm*/) +{ + if (!this->dataPtr->controller_manager_) { + return; + } + static bool warned{false}; + if (!warned) { + rclcpp::Duration gazebo_period(_info.dt); + + // Check the period against the simulation period + if (this->dataPtr->control_period_ < _info.dt) { + RCLCPP_ERROR_STREAM( + this->dataPtr->node_->get_logger(), + "Desired controller update period (" << this->dataPtr->control_period_.seconds() << + " s) is faster than the gazebo simulation period (" << + gazebo_period.seconds() << " s)."); + } else if (this->dataPtr->control_period_ > gazebo_period) { + RCLCPP_WARN_STREAM( + this->dataPtr->node_->get_logger(), + " Desired controller update period (" << this->dataPtr->control_period_.seconds() << + " s) is slower than the gazebo simulation period (" << + gazebo_period.seconds() << " s)."); + } + warned = true; + } + + rclcpp::Time sim_time_ros(std::chrono::duration_cast( + _info.simTime).count(), RCL_ROS_TIME); + rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; + // Always set commands on joints, otherwise at low control frequencies the joints tremble + // as they are updated at a fraction of gazebo sim time + this->dataPtr->controller_manager_->write(sim_time_ros, sim_period); +} + +////////////////////////////////////////////////// +void GazeboSimROS2ControlPlugin::PostUpdate( + const sim::UpdateInfo & _info, + const sim::EntityComponentManager & /*_ecm*/) +{ + if (!this->dataPtr->controller_manager_) { + return; + } + // Get the simulation time and period + rclcpp::Time sim_time_ros(std::chrono::duration_cast( + _info.simTime).count(), RCL_ROS_TIME); + rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; + + if (sim_period >= this->dataPtr->control_period_) { + this->dataPtr->last_update_sim_time_ros_ = sim_time_ros; + auto gz_controller_manager = + std::dynamic_pointer_cast( + this->dataPtr->controller_manager_); + this->dataPtr->controller_manager_->read(sim_time_ros, sim_period); + this->dataPtr->controller_manager_->update(sim_time_ros, sim_period); + } +} +} // namespace gz_ros2_control + +#ifdef GZ_HEADERS +GZ_ADD_PLUGIN( + gz_ros2_control::GazeboSimROS2ControlPlugin, + sim::System, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemConfigure, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPreUpdate, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPostUpdate) +GZ_ADD_PLUGIN_ALIAS( + gz_ros2_control::GazeboSimROS2ControlPlugin, + "ign_ros2_control::IgnitionROS2ControlPlugin") +#else +IGNITION_ADD_PLUGIN( + gz_ros2_control::GazeboSimROS2ControlPlugin, + sim::System, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemConfigure, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPreUpdate, + gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPostUpdate) +IGNITION_ADD_PLUGIN_ALIAS( + gz_ros2_control::GazeboSimROS2ControlPlugin, + "ign_ros2_control::IgnitionROS2ControlPlugin") +#endif diff --git a/hardwares/gz_quadruped_hardware/src/gz_system.cpp b/hardwares/gz_quadruped_hardware/src/gz_system.cpp index 759a0fd..6d7f278 100644 --- a/hardwares/gz_quadruped_hardware/src/gz_system.cpp +++ b/hardwares/gz_quadruped_hardware/src/gz_system.cpp @@ -202,7 +202,7 @@ namespace gz_quadruped_hardware std::map& enableJoints, const hardware_interface::HardwareInfo& hardware_info, sim::EntityComponentManager& _ecm, - unsigned int update_rate) + int& update_rate) { this->dataPtr = std::make_unique(); this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); @@ -272,24 +272,6 @@ namespace gz_quadruped_hardware // Accept this joint and continue configuration RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name); - // check if joint is mimicked - auto it = std::find_if( - hardware_info.mimic_joints.begin(), - hardware_info.mimic_joints.end(), - [j](const hardware_interface::MimicJoint& mj) - { - return mj.joint_index == j; - }); - - if (it != hardware_info.mimic_joints.end()) - { - RCLCPP_INFO_STREAM( - this->nh_->get_logger(), - "Joint '" << joint_name << "'is mimicking joint '" << - this->dataPtr->joints_[it->mimicked_joint_index].name << - "' with multiplier: " << it->multiplier << " and offset: " << it->offset); - } - RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); auto get_initial_value = @@ -536,9 +518,9 @@ namespace gz_quadruped_hardware ftData->name = _name->Data(); ftData->sim_ft_sensors_ = _entity; this->dataPtr->state_interfaces_.emplace_back( - "foot_force", - ftData->name, - &ftData->foot_effort); + "foot_force", + ftData->name, + &ftData->foot_effort); this->dataPtr->ft_sensors_.push_back(ftData); return true; }); @@ -680,6 +662,60 @@ namespace gz_quadruped_hardware return hardware_interface::return_type::OK; } + hardware_interface::return_type + GazeboSimSystem::perform_command_mode_switch( + const std::vector& start_interfaces, + const std::vector& stop_interfaces) + { + for (unsigned int j = 0; j < this->dataPtr->joints_.size(); j++) + { + for (const std::string& interface_name : stop_interfaces) + { + // Clear joint control method bits corresponding to stop interfaces + if (interface_name == (this->dataPtr->joints_[j].name + "/" + + hardware_interface::HW_IF_POSITION)) + { + this->dataPtr->joints_[j].joint_control_method &= + static_cast(VELOCITY & EFFORT); + } + else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_VELOCITY)) + { + this->dataPtr->joints_[j].joint_control_method &= + static_cast(POSITION & EFFORT); + } + else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_EFFORT)) + { + this->dataPtr->joints_[j].joint_control_method &= + static_cast(POSITION & VELOCITY); + } + } + + // Set joint control method bits corresponding to start interfaces + for (const std::string& interface_name : start_interfaces) + { + if (interface_name == (this->dataPtr->joints_[j].name + "/" + + hardware_interface::HW_IF_POSITION)) + { + this->dataPtr->joints_[j].joint_control_method |= POSITION; + } + else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_VELOCITY)) + { + this->dataPtr->joints_[j].joint_control_method |= VELOCITY; + } + else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT + hardware_interface::HW_IF_EFFORT)) + { + this->dataPtr->joints_[j].joint_control_method |= EFFORT; + } + } + } + + return hardware_interface::return_type::OK; + } + hardware_interface::return_type GazeboSimSystem::write( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)