Skip to content

Commit

Permalink
Make topics configurable for joint controllers (#584)
Browse files Browse the repository at this point in the history
Signed-off-by: Louise Poubel <[email protected]>
chapulina authored Feb 9, 2021
1 parent 4eda53f commit ac10892
Showing 3 changed files with 12 additions and 3 deletions.
7 changes: 4 additions & 3 deletions examples/worlds/joint_position_controller.sdf
Original file line number Diff line number Diff line change
@@ -4,9 +4,9 @@
Try sending joint position commands:
ign topic -t "/model/joint_position_controller_demo/joint/j1/0/cmd_pos" -m ignition.msgs.Double -p "data: -1.0"
ign topic -t "/rotor_cmd" -m ignition.msgs.Double -p "data: -1.0"
ign topic -t "/model/joint_position_controller_demo/joint/j1/0/cmd_pos" -m ignition.msgs.Double -p "data: 1.0"
ign topic -t "/rotor_cmd" -m ignition.msgs.Double -p "data: 1.0"
-->
<sdf version="1.6">
<world name="default">
@@ -45,7 +45,7 @@
<property type="double" key="height">72</property>
<property type="double" key="width">121</property>
<property type="double" key="z">1</property>

<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="left" target="left"/>
@@ -156,6 +156,7 @@
filename="ignition-gazebo-joint-position-controller-system"
name="ignition::gazebo::systems::JointPositionController">
<joint_name>j1</joint_name>
<topic>rotor_cmd</topic>
<p_gain>1</p_gain>
<i_gain>0.1</i_gain>
<d_gain>0.01</d_gain>
4 changes: 4 additions & 0 deletions src/systems/joint_controller/JointController.cc
Original file line number Diff line number Diff line change
@@ -133,6 +133,10 @@ void JointController::Configure(const Entity &_entity,
// Subscribe to commands
std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/joint/" +
this->dataPtr->jointName + "/cmd_vel"};
if (_sdf->HasElement("topic"))
{
topic = _sdf->Get<std::string>("topic");
}
this->dataPtr->node.Subscribe(topic, &JointControllerPrivate::OnCmdVel,
this->dataPtr.get());

Original file line number Diff line number Diff line change
@@ -150,6 +150,10 @@ void JointPositionController::Configure(const Entity &_entity,
std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) +
"/joint/" + this->dataPtr->jointName + "/" +
std::to_string(this->dataPtr->jointIndex) + "/cmd_pos"};
if (_sdf->HasElement("topic"))
{
topic = _sdf->Get<std::string>("topic");
}
this->dataPtr->node.Subscribe(
topic, &JointPositionControllerPrivate::OnCmdPos, this->dataPtr.get());

0 comments on commit ac10892

Please sign in to comment.