diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h index e89443b..90ff04d 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h @@ -97,6 +97,7 @@ namespace ocs2::legged_robot std::unique_ptr getSelfCollisionConstraint(const PinocchioInterface& pinocchioInterface, const std::string& taskFile, + const std::string& urdf_file, const std::string& prefix, bool verbose); ModelSettings model_settings_; diff --git a/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp b/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp index 550e91f..61e8b08 100644 --- a/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp +++ b/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp @@ -18,8 +18,14 @@ namespace ocs2::legged_robot ctrl_component_(ctrl_component), node_(ctrl_component->node_) { - node_->declare_parameter("default_kp", default_kp_); - node_->declare_parameter("default_kd", default_kd_); + if (!node_->has_parameter("default_kp")) + { + node_->declare_parameter("default_kp", default_kp_); + } + if (!node_->has_parameter("default_kd")) + { + node_->declare_parameter("default_kd", default_kd_); + } default_kp_ = node_->get_parameter("default_kp").as_double(); default_kd_ = node_->get_parameter("default_kd").as_double(); diff --git a/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp b/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp index 3bc34e7..31d4300 100644 --- a/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp +++ b/controllers/ocs2_quadruped_controller/src/interface/LeggedInterface.cpp @@ -160,7 +160,8 @@ namespace ocs2::legged_robot // Self-collision avoidance constraint problem_ptr_->stateSoftConstraintPtr->add("selfCollision", getSelfCollisionConstraint( - *pinocchio_interface_ptr_, task_file, "selfCollision", verbose)); + *pinocchio_interface_ptr_, task_file, urdf_file, "selfCollision", + verbose)); // Setup Problem PreComputation problem_ptr_->preComputationPtr = std::make_unique( @@ -390,6 +391,7 @@ namespace ocs2::legged_robot std::unique_ptr LeggedInterface::getSelfCollisionConstraint(const PinocchioInterface& pinocchioInterface, const std::string& taskFile, + const std::string& urdf_file, const std::string& prefix, bool verbose) { @@ -413,7 +415,7 @@ namespace ocs2::legged_robot loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionLinkPairs", collisionLinkPairs, verbose); geometry_interface_ptr_ = std::make_unique( - pinocchioInterface, collisionLinkPairs, collisionObjectPairs); + pinocchioInterface, urdf_file, collisionLinkPairs, collisionObjectPairs); if (verbose) { std::cerr << " #### =============================================================================\n"; diff --git a/descriptions/unitree/go2_description/urdf/robot.urdf b/descriptions/unitree/go2_description/urdf/robot.urdf index dbdf128..096b401 100644 --- a/descriptions/unitree/go2_description/urdf/robot.urdf +++ b/descriptions/unitree/go2_description/urdf/robot.urdf @@ -1,798 +1,520 @@ - - - - - - + + - - - - - - - - - - - - - + + - + + + + + + + + + + + + + + - + + + + + + - + + - - - hardware_unitree_mujoco/HardwareUnitree - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - - - - - - + + + + + + - + + + + + + + + + + + + + + + + + + + + - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0 - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - + + - - - - - - - - - - - - - - - - - - - - - 0 - - + + - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0 - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - - - - - + + + - - - - - - - - - - - - - - - - - - - - - - - - + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - + + - - - + + - - - - - - - - - - - - - - - - - - - - - 0 - - + + - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp b/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp index 1cbc43b..f291d4f 100644 --- a/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp +++ b/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp @@ -45,7 +45,6 @@ #include #include -#include #include @@ -90,8 +89,7 @@ namespace gz_quadruped_hardware rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); /// \brief Interface loader - std::shared_ptr> + std::shared_ptr> robot_hw_sim_loader_{nullptr}; /// \brief Controller manager @@ -106,7 +104,7 @@ namespace gz_quadruped_hardware /// \brief Last time the update method was called rclcpp::Time last_update_sim_time_ros_ = - rclcpp::Time((int64_t)0, RCL_ROS_TIME); + rclcpp::Time(static_cast(0), RCL_ROS_TIME); /// \brief ECM pointer sim::EntityComponentManager* ecm{nullptr}; @@ -220,7 +218,7 @@ namespace gz_quadruped_hardware { auto f = parameters_client->get_parameters({this->robot_description_}); f.wait(); - std::vector values = f.get(); + const std::vector& values = f.get(); urdf_string = values[0].as_string(); } catch (const std::exception& e) @@ -286,7 +284,7 @@ namespace gz_quadruped_hardware } // Get params from SDF - std::string paramFileName = _sdf->Get("parameters"); + auto paramFileName = _sdf->Get("parameters"); if (paramFileName.empty()) { @@ -297,7 +295,7 @@ namespace gz_quadruped_hardware } // Get params from SDF - std::string robot_param_node = _sdf->Get("robot_param_node"); + auto robot_param_node = _sdf->Get("robot_param_node"); if (!robot_param_node.empty()) { this->dataPtr->robot_description_node_ = robot_param_node; @@ -306,7 +304,7 @@ namespace gz_quadruped_hardware logger, "robot_param_node is %s", this->dataPtr->robot_description_node_.c_str()); - std::string robot_description = _sdf->Get("robot_param"); + auto robot_description = _sdf->Get("robot_param"); if (!robot_description.empty()) { this->dataPtr->robot_description_ = robot_description; @@ -322,8 +320,8 @@ namespace gz_quadruped_hardware sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters"); while (argument_sdf) { - std::string argument = argument_sdf->Get(); - arguments.push_back(RCL_PARAM_FILE_FLAG); + auto argument = argument_sdf->Get(); + arguments.emplace_back(RCL_PARAM_FILE_FLAG); arguments.push_back(argument); argument_sdf = argument_sdf->GetNextElement("parameters"); } @@ -361,11 +359,11 @@ namespace gz_quadruped_hardware { sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); - arguments.push_back(RCL_ROS_ARGS_FLAG); + arguments.emplace_back(RCL_ROS_ARGS_FLAG); while (argument_sdf) { auto argument = argument_sdf->Get(); - arguments.push_back(RCL_REMAP_FLAG); + arguments.emplace_back(RCL_REMAP_FLAG); arguments.push_back(argument); argument_sdf = argument_sdf->GetNextElement("remapping"); } @@ -406,13 +404,14 @@ namespace gz_quadruped_hardware const auto rb_arg = std::string("robot_description:=") + std::regex_replace( urdf_string, comment_pattern, ""); - arguments.push_back(RCL_PARAM_FLAG); + arguments.emplace_back(RCL_PARAM_FLAG); arguments.push_back(rb_arg); std::vector argv; + argv.reserve(arguments.size()); for (const auto& arg : arguments) { - argv.push_back(reinterpret_cast(arg.data())); + argv.push_back(arg.data()); } // set the arguments into rcl context @@ -445,7 +444,7 @@ namespace gz_quadruped_hardware _entity, _ecm); - if (enabledJoints.size() == 0) + if (enabledJoints.empty()) { RCLCPP_DEBUG_STREAM( this->dataPtr->node_->get_logger(), @@ -483,10 +482,9 @@ namespace gz_quadruped_hardware } try { - this->dataPtr->robot_hw_sim_loader_.reset( - new pluginlib::ClassLoader( - "gz_quadruped_hardware", - "gz_quadruped_hardware::GazeboSimSystemInterface")); + this->dataPtr->robot_hw_sim_loader_ = std::make_shared>( + "gz_quadruped_hardware", + "gz_quadruped_hardware::GazeboSimSystemInterface"); } catch (pluginlib::LibraryLoadException& ex) { @@ -496,9 +494,9 @@ namespace gz_quadruped_hardware return; } - for (unsigned int i = 0; i < control_hardware_info.size(); ++i) + for (auto& i : control_hardware_info) { - std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_class_type; + std::string robot_hw_sim_type_str_ = i.hardware_class_type; std::unique_ptr gzSimSystem; RCLCPP_DEBUG( this->dataPtr->node_->get_logger(), "Load hardware interface %s ...", @@ -520,7 +518,7 @@ namespace gz_quadruped_hardware if (!gzSimSystem->initSim( this->dataPtr->node_, enabledJoints, - control_hardware_info[i], + i, _ecm, this->dataPtr->update_rate)) { @@ -532,28 +530,27 @@ namespace gz_quadruped_hardware 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]); + resource_manager_->import_component(std::move(gzSimSystem), 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); + resource_manager_->set_component_state(i.name, state); } // Create the controller manager RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager"); rclcpp::NodeOptions options = controller_manager::get_cm_node_options(); - arguments.push_back("-r"); + arguments.emplace_back("-r"); arguments.push_back("__node:=" + controllerManagerNodeName); - arguments.push_back("-r"); + arguments.emplace_back("-r"); arguments.push_back("__ns:=" + ns); options.arguments(arguments); - this->dataPtr->controller_manager_.reset( - new controller_manager::ControllerManager( - std::move(resource_manager_), - this->dataPtr->executor_, - controllerManagerNodeName, - this->dataPtr->node_->get_namespace())); + this->dataPtr->controller_manager_ = std::make_shared( + 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")) diff --git a/hardwares/gz_quadruped_hardware/src/gz_ros2_control_plugin.cpp b/hardwares/gz_quadruped_hardware/src/gz_ros2_control_plugin.cpp deleted file mode 100644 index d4c51e2..0000000 --- a/hardwares/gz_quadruped_hardware/src/gz_ros2_control_plugin.cpp +++ /dev/null @@ -1,611 +0,0 @@ -// 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