diff --git a/doc/index.rst b/doc/index.rst index e24a2308..ca0a4c82 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -155,6 +155,7 @@ The *gazebo_ros2_control* ```` tag also has the following optional child * ````: The location of the ``robot_description`` (URDF) on the parameter server, defaults to ``robot_description`` * ````: Name of the node where the ``robot_param`` is located, defaults to ``robot_state_publisher`` * ````: 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 gazebo_ros2_control Behavior ----------------------------------------------------------- diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index b0a8bb07..2008cacd 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -182,6 +182,12 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } else { impl_->robot_description_node_ = "robot_state_publisher"; // default } + // Hold joints if no control mode is active? + bool hold_joints = true; // default + if (sdf->HasElement("hold_joints")) { + hold_joints = + sdf->GetElement("hold_joints")->Get(); + } // There's currently no direct way to set parameters to the plugin's node // So we have to parse the plugin file manually and set it to the node's context. @@ -306,6 +312,23 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element RCLCPP_DEBUG( impl_->model_nh_->get_logger(), "Loaded hardware interface %s!", robot_hw_sim_type_str_.c_str()); + try { + node_ros2->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints)); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has already been declared, %s", + e.what()); + } catch (const rclcpp::exceptions::InvalidParametersException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' has invalid name, %s", e.what()); + } catch (const rclcpp::exceptions::InvalidParameterValueException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value is invalid, %s", e.what()); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + RCLCPP_ERROR( + impl_->model_nh_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s", + e.what()); + } if (!gazeboSystem->initSim( node_ros2, impl_->parent_model_, diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index a9292bd9..af3e73be 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -103,6 +103,9 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief mapping of mimicked joints to index of joint they mimic std::vector mimic_joints_; + + // Should hold the joints if no control_mode is active + bool hold_joints_ = true; }; namespace gazebo_ros2_control @@ -128,6 +131,30 @@ bool GazeboSystem::initSim( return false; } + 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); + registerJoints(hardware_info, parent_model); registerSensors(hardware_info, parent_model); @@ -601,7 +628,7 @@ hardware_interface::return_type GazeboSystem::write( this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]); } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]); - } else if (this->dataPtr->is_joint_actuated_[j]) { + } else if (this->dataPtr->is_joint_actuated_[j] && this->dataPtr->hold_joints_) { // Fallback case is a velocity command of zero (only for actuated joints) this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); }