From d4324093196b45c48cdb857aa9f6bca742acc65f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 1 Apr 2024 21:33:53 +0200 Subject: [PATCH 1/8] Override RM to enable simulator-specific functionality. Don't use raw pointers for passing update rate. --- .../include/gz_ros2_control/gz_system.hpp | 2 +- .../gz_ros2_control/gz_system_interface.hpp | 2 +- .../src/gz_ros2_control_plugin.cpp | 276 +++++++----------- gz_ros2_control/src/gz_system.cpp | 15 +- 4 files changed, 116 insertions(+), 179 deletions(-) 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..7b0d6479 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; + const 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 745864d3..23dc4f4d 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 @@ -93,7 +93,7 @@ class GazeboSimSystemInterface std::map & joints, const hardware_interface::HardwareInfo & hardware_info, sim::EntityComponentManager & _ecm, - int & update_rate) = 0; + const 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 b4769175..00dea774 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -56,13 +56,90 @@ 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, const 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 @@ -101,14 +178,6 @@ class GazeboSimROS2ControlPluginPrivate std::shared_ptr controller_manager_{nullptr}; - /// \brief String with the robot description param_name - // TODO(ahcorde): Add param in plugin tag - std::string robot_description_ = "robot_description"; - - /// \brief String with the name of the node that contains the robot_description - // TODO(ahcorde): Add param in plugin tag - 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); @@ -185,61 +254,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()) @@ -324,9 +338,9 @@ void GazeboSimROS2ControlPlugin::Configure( ns = '/' + ns; } - if (ns.length() > 1) { - this->dataPtr->robot_description_node_ = ns + "/" + this->dataPtr->robot_description_node_; - } + // if (ns.length() > 1) { + // this->dataPtr->robot_description_node_ = ns + "/" + this->dataPtr->robot_description_node_; + // } } // Get list of remapping rules from SDF @@ -384,102 +398,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"); @@ -491,15 +431,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)))); @@ -508,6 +440,12 @@ 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 87b085ff..5bff0ba3 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -170,7 +170,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; @@ -193,7 +193,7 @@ bool GazeboSimSystem::initSim( std::map & enableJoints, const hardware_interface::HardwareInfo & hardware_info, sim::EntityComponentManager & _ecm, - int & update_rate) + const unsigned int update_rate) { this->dataPtr = std::make_unique(); this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); @@ -202,7 +202,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(); @@ -520,10 +520,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; @@ -677,7 +676,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; @@ -742,7 +741,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( From 567ec151b618552ef4a4c646b01bbdf750c6a426 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 1 Apr 2024 21:34:47 +0200 Subject: [PATCH 2/8] Fix formatting. --- gz_ros2_control/src/gz_ros2_control_plugin.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index 00dea774..2c706886 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -75,7 +75,9 @@ class GZResourceManager : public hardware_interface::ResourceManager GZResourceManager(const GZResourceManager &) = delete; // Called from Controller Manager when robot description is initialized from callback - bool load_and_initialize_components(const std::string & urdf, const unsigned int update_rate) override + bool load_and_initialize_components( + const std::string & urdf, + const unsigned int update_rate) override { components_are_loaded_and_initialized_ = true; @@ -337,10 +339,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 @@ -442,7 +440,9 @@ void GazeboSimROS2ControlPlugin::Configure( // 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..."); + RCLCPP_WARN( + this->dataPtr->node_->get_logger(), + "Waiting RM to load and initialize hardware..."); std::this_thread::sleep_for(std::chrono::microseconds(2000000)); } From 249bc6a541ed55bf76f16557cf677f47c0545892 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 17 Jun 2024 20:24:57 +0200 Subject: [PATCH 3/8] Remove const argument. --- gz_ros2_control/include/gz_ros2_control/gz_system.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 7b0d6479..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, - const unsigned int update_rate) override; + unsigned int update_rate) override; private: // Register a sensor (for now just IMUs) From 91a471226045e7581eeb8a7aa173b7ba7adcd16a Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 17 Jun 2024 20:25:09 +0200 Subject: [PATCH 4/8] Remove const argument. Co-authored-by: Sai Kishor Kothakota --- gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 23dc4f4d..f0b5ab7d 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 @@ -93,7 +93,7 @@ class GazeboSimSystemInterface std::map & joints, const hardware_interface::HardwareInfo & hardware_info, sim::EntityComponentManager & _ecm, - const unsigned int update_rate) = 0; + unsigned int update_rate) = 0; // Methods used to control a joint. enum ControlMethod_ From 230ce256d277f0ff2c784ea7c3c60ff1f8f3f02e Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 17 Jun 2024 20:25:42 +0200 Subject: [PATCH 5/8] make upate rate not const. Co-authored-by: Sai Kishor Kothakota --- gz_ros2_control/src/gz_ros2_control_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index 2c706886..8c818d6f 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -77,7 +77,7 @@ class GZResourceManager : public hardware_interface::ResourceManager // Called from Controller Manager when robot description is initialized from callback bool load_and_initialize_components( const std::string & urdf, - const unsigned int update_rate) override + unsigned int update_rate) override { components_are_loaded_and_initialized_ = true; From b1aa3bde067753b578328b556cc76a108970d1e6 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 17 Jun 2024 20:26:54 +0200 Subject: [PATCH 6/8] Update gz_ros2_control_plugin.cpp --- gz_ros2_control/src/gz_ros2_control_plugin.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index 8c818d6f..7ffb0039 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 From 47fb1fda5641d0b441cb0ed4a54033c8a215004a Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 17 Jun 2024 20:27:10 +0200 Subject: [PATCH 7/8] Update gz_ros2_control/src/gz_system.cpp Co-authored-by: Sai Kishor Kothakota --- gz_ros2_control/src/gz_system.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 11d8ca01..739c29eb 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -182,7 +182,7 @@ bool GazeboSimSystem::initSim( std::map & enableJoints, const hardware_interface::HardwareInfo & hardware_info, sim::EntityComponentManager & _ecm, - const unsigned int update_rate) + unsigned int update_rate) { this->dataPtr = std::make_unique(); this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); From b336cd718f1470a201560555dc16c2f5ac0004ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 24 Jun 2024 19:27:43 +0200 Subject: [PATCH 8/8] Fix after merge. --- gz_ros2_control/src/gz_ros2_control_plugin.cpp | 16 ---------------- gz_ros2_control/src/gz_system.cpp | 2 +- 2 files changed, 1 insertion(+), 17 deletions(-) diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index 1c121c55..60161d5d 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -292,22 +292,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()); diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index b63b169e..2f48f954 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -689,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(