diff --git a/doc/index.rst b/doc/index.rst index d47d016e..8ca86ac1 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -153,6 +153,7 @@ robot hardware interfaces between *ros2_control* and Gazebo. The *gz_ros2_control* ```` tag also has the following optional child elements: * ````: YAML file with the configuration of the controllers +* ````: if set to true (default), it will hold the joints' position if their interface was not claimed, e.g., the controller hasn't been activated yet. Default gz_ros2_control Behavior ----------------------------------------------------------- diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp index ab9aacaf..f2585935 100644 --- a/gz_ros2_control/src/gz_ros2_control_plugin.cpp +++ b/gz_ros2_control/src/gz_ros2_control_plugin.cpp @@ -299,6 +299,13 @@ void GazeboSimROS2ControlPlugin::Configure( std::string controllerManagerNodeName{"controller_manager"}; std::string ns = "/"; + // Hold joints if no control mode is active? + bool hold_joints = true; // default + if (sdfPtr->HasElement("hold_joints")) { + hold_joints = + sdfPtr->GetElement("hold_joints")->Get(); + } + if (sdfPtr->HasElement("ros")) { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); @@ -417,6 +424,27 @@ void GazeboSimROS2ControlPlugin::Configure( 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, diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index ed54044e..975ef631 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -175,6 +175,9 @@ class gz_ros2_control::GazeboSimSystemPrivate /// \brief Gain which converts position error to a velocity command double position_proportional_gain_; + + // Should hold the joints if no control_mode is active + bool hold_joints_ = true; }; namespace gz_ros2_control @@ -196,6 +199,31 @@ bool GazeboSimSystem::initSim( this->dataPtr->update_rate = &update_rate; + try { + this->dataPtr->hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool(); + } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' not initialized, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' not declared, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); + } catch (rclcpp::ParameterTypeException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' has wrong type: %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); + } + RCLCPP_DEBUG_STREAM( + this->nh_->get_logger(), "hold_joints (system): " << this->dataPtr->hold_joints_ << std::endl); + + RCLCPP_DEBUG(this->nh_->get_logger(), "n_dof_ %lu", this->dataPtr->n_dof_); this->dataPtr->joints_.resize(this->dataPtr->n_dof_); @@ -658,7 +686,7 @@ hardware_interface::return_type GazeboSimSystem::write( *jointEffortCmd = sim::components::JointForceCmd( {this->dataPtr->joints_[i].joint_effort_cmd}); } - } else if (this->dataPtr->joints_[i].is_actuated) { + } else if (this->dataPtr->joints_[i].is_actuated && this->dataPtr->hold_joints_) { // Fallback case is a velocity command of zero (only for actuated joints) double target_vel = 0.0; auto vel =