diff --git a/examples/worlds/joint_position_controller.sdf b/examples/worlds/joint_position_controller.sdf index 6d2ad62e83..b2bd104509 100644 --- a/examples/worlds/joint_position_controller.sdf +++ b/examples/worlds/joint_position_controller.sdf @@ -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" --> @@ -45,7 +45,7 @@ 72 121 1 - + floating @@ -156,6 +156,7 @@ filename="ignition-gazebo-joint-position-controller-system" name="ignition::gazebo::systems::JointPositionController"> j1 + rotor_cmd 1 0.1 0.01 diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 42990d4447..695ee62d6d 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -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("topic"); + } this->dataPtr->node.Subscribe(topic, &JointControllerPrivate::OnCmdVel, this->dataPtr.get()); diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index c048ff0e23..691336d901 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -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("topic"); + } this->dataPtr->node.Subscribe( topic, &JointPositionControllerPrivate::OnCmdPos, this->dataPtr.get());