[DiffDrive] add enable/disable (#772)

* 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]>

src/systems/diff_drive/DiffDrive.cc

@@ -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
@@ -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;
 
@@ -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"))
@@ -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,

test/integration/diff_drive_system.cc

@@ -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)
 {