Skip to content

Commit

Permalink
[DiffDrive] add enable/disable (#772)
Browse files Browse the repository at this point in the history
* add enable/disable diffdrive

Signed-off-by: Guillaume Doisy <[email protected]>

* remove debug

Signed-off-by: Guillaume Doisy <[email protected]>

* do not subscribe to enable if topic is empty

Signed-off-by: Guillaume Doisy <[email protected]>

* add test

Signed-off-by: Guillaume Doisy <[email protected]>

* lint and style

Signed-off-by: Guillaume Doisy <[email protected]>

* change enable type to bool and renamed to enabled

Signed-off-by: Guillaume Doisy <[email protected]>
  • Loading branch information
doisyg authored and chapulina committed Aug 4, 2021
1 parent 0e9616d commit 3424980
Show file tree
Hide file tree
Showing 2 changed files with 156 additions and 1 deletion.
33 changes: 32 additions & 1 deletion src/systems/diff_drive/DiffDrive.cc
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,10 @@ class ignition::gazebo::systems::DiffDrivePrivate
/// \param[in] _msg Velocity message
public: void OnCmdVel(const ignition::msgs::Twist &_msg);

/// \brief Callback for enable/disable subscription
/// \param[in] _msg Boolean message
public: void OnEnable(const ignition::msgs::Boolean &_msg);

/// \brief Update odometry and publish an odometry message.
/// \param[in] _info System update information.
/// \param[in] _ecm The EntityComponentManager of the given simulation
Expand Down Expand Up @@ -136,6 +140,9 @@ class ignition::gazebo::systems::DiffDrivePrivate
/// \brief Last target velocity requested.
public: msgs::Twist targetVel;

/// \brief Enable/disable state of the controller.
public: bool enabled;

/// \brief A mutex to protect the target velocity command.
public: std::mutex mutex;

Expand Down Expand Up @@ -257,6 +264,14 @@ void DiffDrive::Configure(const Entity &_entity,
this->dataPtr->node.Subscribe(topic, &DiffDrivePrivate::OnCmdVel,
this->dataPtr.get());

// Subscribe to enable/disable
std::string enableTopic{"/model/" + this->dataPtr->model.Name(_ecm) +
"/enable"};

this->dataPtr->node.Subscribe(enableTopic, &DiffDrivePrivate::OnEnable,
this->dataPtr.get());
this->dataPtr->enabled = true;

std::string odomTopic{"/model/" + this->dataPtr->model.Name(_ecm) +
"/odometry"};
if (_sdf->HasElement("odom_topic"))
Expand Down Expand Up @@ -521,7 +536,23 @@ void DiffDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info,
void DiffDrivePrivate::OnCmdVel(const msgs::Twist &_msg)
{
std::lock_guard<std::mutex> lock(this->mutex);
this->targetVel = _msg;
if (this->enabled)
{
this->targetVel = _msg;
}
}

//////////////////////////////////////////////////
void DiffDrivePrivate::OnEnable(const msgs::Boolean &_msg)
{
std::lock_guard<std::mutex> lock(this->mutex);
this->enabled = _msg.data();
if (!this->enabled)
{
math::Vector3d zeroVector{0, 0, 0};
msgs::Set(this->targetVel.mutable_linear(), zeroVector);
msgs::Set(this->targetVel.mutable_angular(), zeroVector);
}
}

IGNITION_ADD_PLUGIN(DiffDrive,
Expand Down
124 changes: 124 additions & 0 deletions test/integration/diff_drive_system.cc
Original file line number Diff line number Diff line change
Expand Up @@ -335,6 +335,130 @@ TEST_P(DiffDriveTest, SkidPublishCmd)
#endif
}

/////////////////////////////////////////////////
TEST_P(DiffDriveTest, EnableDisableCmd)
{
// Start server
ServerConfig serverConfig;
serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/diff_drive_skid.sdf");

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

server.SetUpdatePeriod(0ns);

// Create a system that records the vehicle poses
test::Relay testSystem;

std::vector<math::Pose3d> poses;
testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &,
const gazebo::EntityComponentManager &_ecm)
{
auto id = _ecm.EntityByComponents(
components::Model(),
components::Name("vehicle"));
EXPECT_NE(kNullEntity, id);

auto poseComp = _ecm.Component<components::Pose>(id);
ASSERT_NE(nullptr, poseComp);

poses.push_back(poseComp->Data());
});
server.AddSystem(testSystem.systemPtr);

// Run server and check that vehicle didn't move
server.Run(true, 1000, false);

EXPECT_EQ(1000u, poses.size());

for (const auto &pose : poses)
{
EXPECT_EQ(poses[0], pose);
}

// Publish command and check that vehicle moved
double period{1.0};
double lastMsgTime{1.0};
std::vector<math::Pose3d> odomPoses;
std::function<void(const msgs::Odometry &)> odomCb =
[&](const msgs::Odometry &_msg)
{
ASSERT_TRUE(_msg.has_header());
ASSERT_TRUE(_msg.header().has_stamp());

double msgTime =
static_cast<double>(_msg.header().stamp().sec()) +
static_cast<double>(_msg.header().stamp().nsec()) * 1e-9;

EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period);
lastMsgTime = msgTime;

odomPoses.push_back(msgs::Convert(_msg.pose()));
};

transport::Node node;
auto pub = node.Advertise<msgs::Twist>("/model/vehicle/cmd_vel");
node.Subscribe("/model/vehicle/odometry", odomCb);

msgs::Twist msg;
msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0));
msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.0));

pub.Publish(msg);

server.Run(true, 3000, false);

// Poses for 4s
EXPECT_EQ(4000u, poses.size());

// Disable controller
auto pub_enable = node.Advertise<msgs::Boolean>("/model/vehicle/enable");

msgs::Boolean msg_enable;
msg_enable.set_data(false);

pub_enable.Publish(msg_enable);

// Run for 2s and expect no movement
server.Run(true, 2000, false);

EXPECT_EQ(6000u, poses.size());

// Re-enable controller
msg_enable.set_data(true);

pub_enable.Publish(msg_enable);

pub.Publish(msg);

// Run for 2s and expect movement again
server.Run(true, 2000, false);

EXPECT_EQ(8000u, poses.size());

int sleep = 0;
int maxSleep = 70;
for (; odomPoses.size() < 7 && sleep < maxSleep; ++sleep)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
EXPECT_NE(maxSleep, sleep);

// Odom for 7s
ASSERT_FALSE(odomPoses.empty());
EXPECT_EQ(7u, odomPoses.size());

EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X());

// Should no be moving from 5s to 6s (stopped at 3s and time to slow down)
EXPECT_NEAR(poses[4999].Pos().X(), poses[5999].Pos().X(), tol);

// Should be moving from 6s to 8s
EXPECT_LT(poses[5999].Pos().X(), poses[7999].Pos().X());
}

/////////////////////////////////////////////////
TEST_P(DiffDriveTest, OdomFrameId)
{
Expand Down

0 comments on commit 3424980

Please sign in to comment.