Added warnings when correct set of plugins are not loaded

Signed-off-by: Saurabh Kamat <[email protected]>

src/systems/drive_to_pose_controller/DriveToPoseController.cc

@@ -30,6 +30,11 @@
 #include <gz/transport/Node.hh>
 #include <gz/transport/TopicUtils.hh>
 
+#include <cmath>
+#include <mutex>
+#include <optional>
+#include <string>
+
 using namespace gz;
 using namespace sim;
 using namespace systems;
@@ -135,22 +140,47 @@ void DriveToPoseController::Configure(
   // Topic namespace for publish and subscribe
   std::string topicNamespace = "/model/" + this->dataPtr->model.Name(_ecm);
 
-  // Subscribe to pose topics
+  // Subscribe to odometry pose publisher
   this->dataPtr->node.Subscribe(
-    topicNamespace + "/cmd_pose", &DriveToPoseControllerPrivate::OnCmdPose,
+    topicNamespace + "/pose", &DriveToPoseControllerPrivate::OnCurrentPose,
     this->dataPtr.get());
 
+  std::vector<transport::MessagePublisher> publishers;
+  std::vector<transport::MessagePublisher> subscribers;
+  this->dataPtr->node.TopicInfo(
+    topicNamespace + "/pose", publishers, subscribers);
+  if (publishers.size() < 1)
+  {
+    gzwarn << "Unable to find publisher on /pose topic!" << std::endl;
+    gzwarn << "Make sure OdometryPublisher plugin "
+           << "is loaded through the SDF." << std::endl;
+  }
+
+  // Subscribe to command pose topic
   this->dataPtr->node.Subscribe(
-    topicNamespace + "/pose", &DriveToPoseControllerPrivate::OnCurrentPose,
+    topicNamespace + "/cmd_pose", &DriveToPoseControllerPrivate::OnCmdPose,
     this->dataPtr.get());
 
+
   // Create velocity publisher
   this->dataPtr->velocityPublisher =
     this->dataPtr->node.Advertise<msgs::Twist>(topicNamespace + "/cmd_vel");
+  if (!this->dataPtr->velocityPublisher.HasConnections())
+  {
+    gzwarn << "Unable to find a subscriber on /cmd_vel topic!" << std::endl;
+    gzwarn << "Make sure DiffDrive plugin "
+           << "is loaded through the SDF." << std::endl;
+  }
 
   // Create pose reached publisher
   this->dataPtr->poseReachedPublisher =
     this->dataPtr->node.Advertise<msgs::Pose>(topicNamespace + "/reached_pose");
+
+  gzdbg << "DriveToPoseController initialized with the following parameters:" << std::endl;
+  gzdbg << "linear_p_gain: " << this->dataPtr->linearPGain << std::endl;
+  gzdbg << "angular_p_gain: " << this->dataPtr->angularPGain << std::endl;
+  gzdbg << "linear_deviation: " << this->dataPtr->linearDeviation << std::endl;
+  gzdbg << "angular_deviation: " << this->dataPtr->angularDeviation << std::endl;
 }
 
 //////////////////////////////////////////////////

src/systems/drive_to_pose_controller/DriveToPoseController.hh

@@ -18,11 +18,15 @@
 #ifndef GZ_SIM_SYSTEMS_DRIVETOPOSECONTROLLER_HH_
 #define GZ_SIM_SYSTEMS_DRIVETOPOSECONTROLLER_HH_
 
-#include <memory>
-#include <string>
-
+#include <gz/sim/Entity.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
 #include <gz/sim/System.hh>
 
+#include <sdf/Element.hh>
+
+#include <memory>
+
 namespace gz
 {
 namespace sim

test/integration/drive_to_pose_controller_system.cc

@@ -24,6 +24,10 @@
 #include <gz/math/Pose3.hh>
 #include <gz/transport/Node.hh>
 
+#include <chrono>
+#include <functional>
+#include <string>
+
 #include "gz/sim/Server.hh"
 
 #include "../helpers/EnvTestFixture.hh"
@@ -64,8 +68,6 @@ class DriveToPoseControllerTest
       [&](const msgs::Pose &_msg)
       {
         reachedPose = msgs::Convert(_msg);
-        std::cout << "reached pose: " << reachedPose.X() << ", "
-                  << reachedPose.Y() << std::endl;
       };
 
     // Get the odom pose message
@@ -132,8 +134,9 @@ TEST_F(DriveToPoseControllerTest, CurrentPosePublish)
   math::Pose3d pose(0, 0, 0, 0, 0, 0);
 
   TestPublishCmd(
-    std::string(PROJECT_SOURCE_PATH) +
-    "/test/worlds/drive_to_pose_controller.sdf",
+    gz::common::joinPaths(
+      std::string(PROJECT_SOURCE_PATH),
+      "/test/worlds/drive_to_pose_controller.sdf"),
     "/model/DeliveryBot",
     pose);
 }
@@ -144,9 +147,10 @@ TEST_F(DriveToPoseControllerTest, XCoordinatePublish)
   math::Pose3d pose(1.5, 0, 0, 0, 0, 0);
 
   TestPublishCmd(
-    std::string(PROJECT_SOURCE_PATH) +
-    "/test/worlds/drive_to_pose_controller.sdf",
-    "/model/DeliveryBot",
+    gz::common::joinPaths(
+      std::string(PROJECT_SOURCE_PATH),
+      "/test/worlds/drive_to_pose_controller.sdf"),
+   "/model/DeliveryBot",
     pose);
 }
 
@@ -156,9 +160,10 @@ TEST_F(DriveToPoseControllerTest, YCoordinatePublish)
   math::Pose3d pose(0, 1.5, 0, 0, 0, 0);
 
   TestPublishCmd(
-    std::string(PROJECT_SOURCE_PATH) +
-    "/test/worlds/drive_to_pose_controller.sdf",
-    "/model/DeliveryBot",
+    gz::common::joinPaths(
+      std::string(PROJECT_SOURCE_PATH),
+      "/test/worlds/drive_to_pose_controller.sdf"),
+   "/model/DeliveryBot",
     pose);
 }
 
@@ -168,9 +173,10 @@ TEST_F(DriveToPoseControllerTest, YawPublish)
   math::Pose3d pose(0, 0, 0, 0, 0, -1.57);
 
   TestPublishCmd(
-    std::string(PROJECT_SOURCE_PATH) +
-    "/test/worlds/drive_to_pose_controller.sdf",
-    "/model/DeliveryBot",
+    gz::common::joinPaths(
+      std::string(PROJECT_SOURCE_PATH),
+      "/test/worlds/drive_to_pose_controller.sdf"),
+   "/model/DeliveryBot",
     pose);
 }
 
@@ -180,8 +186,9 @@ TEST_F(DriveToPoseControllerTest, XYCoordinateYawPublish)
   math::Pose3d pose(1.5, -1.5, 0, 0, 0, 1.57);
 
   TestPublishCmd(
-    std::string(PROJECT_SOURCE_PATH) +
-    "/test/worlds/drive_to_pose_controller.sdf",
-    "/model/DeliveryBot",
+    gz::common::joinPaths(
+      std::string(PROJECT_SOURCE_PATH),
+      "/test/worlds/drive_to_pose_controller.sdf"),
+   "/model/DeliveryBot",
     pose);
 }