[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]>
gazebosim · Aug 4, 2021 · 3424980 · 3424980
1 parent 0e9616d
commit 3424980
Show file tree

Hide file tree

Showing 2 changed files with 156 additions and 1 deletion.
diff --git a/src/systems/diff_drive/DiffDrive.cc b/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,

diff --git a/test/integration/diff_drive_system.cc b/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)
 {