Skip to content

Commit

Permalink
Showing 3 changed files with 51 additions and 19 deletions.
37 changes: 18 additions & 19 deletions src/systems/joint_controller/JointController.cc
Original file line number Diff line number Diff line change
@@ -47,9 +47,6 @@ class ignition::gazebo::systems::JointControllerPrivate
/// \brief Joint Entity
public: Entity jointEntity;

/// \brief Joint name
public: std::string jointName;

/// \brief Commanded joint velocity
public: double jointVelCmd;

@@ -89,15 +86,23 @@ void JointController::Configure(const Entity &_entity,
}

// Get params from SDF
this->dataPtr->jointName = _sdf->Get<std::string>("joint_name");

if (this->dataPtr->jointName == "")
auto jointName = _sdf->Get<std::string>("joint_name");
if (jointName.empty())
{
ignerr << "JointController found an empty jointName parameter. "
<< "Failed to initialize.";
return;
}

this->dataPtr->jointEntity = this->dataPtr->model.JointByName(_ecm,
jointName);
if (this->dataPtr->jointEntity == kNullEntity)
{
ignerr << "Joint with name[" << jointName << "] not found. "
<< "The JointController may not control this joint.\n";
return;
}

if (_sdf->HasElement("initial_velocity"))
{
this->dataPtr->jointVelCmd = _sdf->Get<double>("initial_velocity");
@@ -139,11 +144,11 @@ void JointController::Configure(const Entity &_entity,

// Subscribe to commands
std::string topic = transport::TopicUtils::AsValidTopic("/model/" +
this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName +
this->dataPtr->model.Name(_ecm) + "/joint/" + jointName +
"/cmd_vel");
if (topic.empty())
{
ignerr << "Failed to create topic for joint [" << this->dataPtr->jointName
ignerr << "Failed to create topic for joint [" << jointName
<< "]" << std::endl;
return;
}
@@ -155,7 +160,7 @@ void JointController::Configure(const Entity &_entity,
if (topic.empty())
{
ignerr << "Failed to create topic [" << _sdf->Get<std::string>("topic")
<< "]" << " for joint [" << this->dataPtr->jointName
<< "]" << " for joint [" << jointName
<< "]" << std::endl;
return;
}
@@ -173,6 +178,10 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
{
IGN_PROFILE("JointController::PreUpdate");

// If the joint hasn't been identified yet, the plugin is disabled
if (this->dataPtr->jointEntity == kNullEntity)
return;

// \TODO(anyone) Support rewind
if (_info.dt < std::chrono::steady_clock::duration::zero())
{
@@ -181,16 +190,6 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
<< "s]. System may not work properly." << std::endl;
}

// If the joint hasn't been identified yet, look for it
if (this->dataPtr->jointEntity == kNullEntity)
{
this->dataPtr->jointEntity =
this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName);
}

if (this->dataPtr->jointEntity == kNullEntity)
return;

// Nothing left to do if paused.
if (_info.paused)
return;
19 changes: 19 additions & 0 deletions test/integration/joint_controller_system.cc
Original file line number Diff line number Diff line change
@@ -224,3 +224,22 @@ TEST_F(JointControllerTestFixture, JointVelocityCommandWithForce)
EXPECT_NEAR(0, angularVelocity.Y(), 1e-2);
EXPECT_NEAR(testAngVel, angularVelocity.Z(), 1e-2);
}

/////////////////////////////////////////////////
TEST_F(JointControllerTestFixture, InexistentJoint)
{
using namespace std::chrono_literals;

// Start server
ServerConfig serverConfig;
const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
"test", "worlds", "joint_controller_invalid.sdf");
serverConfig.SetSdfFile(sdfFile);

Server server(serverConfig);
EXPECT_FALSE(server.Running());
EXPECT_FALSE(*server.Running(0));

// Run some iterations to make sure nothing explodes
server.Run(true, 100, false);
}
14 changes: 14 additions & 0 deletions test/worlds/joint_controller_invalid.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="default">
<model name="joint_controller_test">
<link name="base_link">
</link>
<plugin
filename="ignition-gazebo-joint-controller-system"
name="ignition::gazebo::systems::JointController">
<joint_name>invalid</joint_name>
</plugin>
</model>
</world>
</sdf>

0 comments on commit 582bc5d

Please sign in to comment.