Skip to content

Commit

Permalink
Add hold_joints parameter (#251)
Browse files Browse the repository at this point in the history
(cherry picked from commit 7682dce)

# Conflicts:
#	gazebo_ros2_control/src/gazebo_system.cpp
  • Loading branch information
christophfroehlich authored and mergify[bot] committed Jul 6, 2024
1 parent 6cc95ed commit f17adc9
Show file tree
Hide file tree
Showing 3 changed files with 55 additions and 0 deletions.
1 change: 1 addition & 0 deletions doc/index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -213,6 +213,7 @@ The *gazebo_ros2_control* ``<plugin>`` tag also has the following optional child
* ``<robot_param>``: The location of the ``robot_description`` (URDF) on the parameter server, defaults to ``robot_description``
* ``<robot_param_node>``: Name of the node where the ``robot_param`` is located, defaults to ``robot_state_publisher``
* ``<parameters>``: YAML file with the configuration of the controllers
* ``<hold_joints>``: 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
-----------------------------------------------------------
Expand Down
23 changes: 23 additions & 0 deletions gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -184,6 +184,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<bool>();
}

// Read urdf from ros parameter server
std::string urdf_string;
Expand Down Expand Up @@ -325,6 +331,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_,
Expand Down
31 changes: 31 additions & 0 deletions gazebo_ros2_control/src/gazebo_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,9 @@ class gazebo_ros2_control::GazeboSystemPrivate

/// \brief mapping of mimicked joints to index of joint they mimic
std::vector<MimicJoint> mimic_joints_;

// Should hold the joints if no control_mode is active
bool hold_joints_ = true;
};

namespace gazebo_ros2_control
Expand All @@ -141,6 +144,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);

Expand Down Expand Up @@ -745,6 +772,7 @@ hardware_interface::return_type GazeboSystem::write(

} else if (this->dataPtr->joint_control_methods_[j] & EFFORT) {
this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]);
<<<<<<< HEAD

} else if (this->dataPtr->joint_control_methods_[j] & VELOCITY_PID) {
double vel_goal = this->dataPtr->joint_velocity_cmd_[j];
Expand All @@ -758,6 +786,9 @@ hardware_interface::return_type GazeboSystem::write(
double cmd = this->dataPtr->pos_pid[j].computeCommand(pos_goal - pos, dt);
this->dataPtr->sim_joints_[j]->SetForce(0, cmd);
} else if (this->dataPtr->is_joint_actuated_[j]) {
=======
} else if (this->dataPtr->is_joint_actuated_[j] && this->dataPtr->hold_joints_) {
>>>>>>> 7682dce (Add `hold_joints` parameter (#251))
// Fallback case is a velocity command of zero (only for actuated joints)
this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
}
Expand Down

0 comments on commit f17adc9

Please sign in to comment.