From ed884dc3bc78a102f73b3dd823df7ab68b34b254 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Tue, 12 Nov 2024 02:49:38 +0800 Subject: [PATCH 01/11] Added drive to pose controller Signed-off-by: Saurabh Kamat --- .../drive_to_pose_controller/CMakeLists.txt | 6 + .../DriveToPoseController.cc | 229 ++++++++++++++++++ .../DriveToPoseController.hh | 67 +++++ 3 files changed, 302 insertions(+) create mode 100644 src/systems/drive_to_pose_controller/CMakeLists.txt create mode 100644 src/systems/drive_to_pose_controller/DriveToPoseController.cc create mode 100644 src/systems/drive_to_pose_controller/DriveToPoseController.hh diff --git a/src/systems/drive_to_pose_controller/CMakeLists.txt b/src/systems/drive_to_pose_controller/CMakeLists.txt new file mode 100644 index 0000000000..a580005389 --- /dev/null +++ b/src/systems/drive_to_pose_controller/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(drive-to-pose-controller + SOURCES + DriveToPoseController.cc + PUBLIC_LINK_LIBS + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} +) \ No newline at end of file diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.cc b/src/systems/drive_to_pose_controller/DriveToPoseController.cc new file mode 100644 index 0000000000..1daed8e2a0 --- /dev/null +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.cc @@ -0,0 +1,229 @@ +#include "DriveToPoseController.hh" + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +using namespace gz; +using namespace sim; +using namespace systems; + +////////////////////////////////////////////////// +class gz::sim::systems::DriveToPoseControllerPrivate +{ + /// \brief Callback for pose command + /// \param[in] _msg Pose message + public: void OnCmdPose(const msgs::Pose_V &_msg); + + /// \brief Callback for odometry message + /// \param[in] _msg Current odometry message + public: void OnCurrentPose(const msgs::Pose_V &_msg); + + /// \brief Calculate velocity and publish a twist message + public: void CalculateVelocity(); + + /// \brief Normalize the angles between -pi to pi + /// \param[in] _angle Angle to normalize + public: void NormalizeAngle(double &_angle); + + /// \brief Gazebo communication node + public: transport::Node node; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief Velocity message publisher. + public: transport::Node::Publisher velocityPublisher; + + /// \brief Pose reached publisher + public: transport::Node::Publisher poseReachedPublisher; + + /// \brief Current pose of the model + public: std::optional currentPose; + + /// \brief Target pose of the model + public: std::optional targetPose; + + /// \brief Proportional gain for angular velocity + public: double angularPGain{2.0}; + + /// \brief Proportional gain for linear velocity + public: double linearPGain{1.0}; + + /// \brief Allowable angular deviation + public: double angularDeviation{0.05}; + + /// \brief Allowable linear deviation + public: double linearDeviation{0.1}; + + /// \brief Mutex to protect current and target poses + public: std::mutex mutex; +}; + +////////////////////////////////////////////////// +DriveToPoseController::DriveToPoseController() + : dataPtr(std::make_unique()) +{ + // Do nothing +} + +////////////////////////////////////////////////// +void DriveToPoseController::Configure( + const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& /*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + if (!this->dataPtr->model.Valid(_ecm)) + { + gzerr << "This plugin should be attached to a model entity. " + << "Initialization failed." << std::endl; + return; + } + + // Get linear P gain if available + if (_sdf->HasElement("linear_p_gain")) + { + this->dataPtr->linearPGain = _sdf->Get("linear_p_gain"); + } + + // Get angular P gain if available + if (_sdf->HasElement("angular_p_gain")) + { + this->dataPtr->angularPGain = _sdf->Get("angular_p_gain"); + } + + // Get allowable linear deviation if available + if (_sdf->HasElement("linear_deviation")) + { + this->dataPtr->linearDeviation = _sdf->Get("linear_deviation"); + } + + // Get allowable angular deviation if available + if (_sdf->HasElement("angular_deviation")) + { + this->dataPtr->angularDeviation = _sdf->Get("angular_deviation"); + } + + // Topic namespace for publish and subscribe + std::string topicNamespace = "/model/" + this->dataPtr->model.Name(_ecm); + + // Subscribe to pose topics + this->dataPtr->node.Subscribe( + topicNamespace + "/cmd_pose", &DriveToPoseControllerPrivate::OnCmdPose, + this->dataPtr.get()); + + this->dataPtr->node.Subscribe( + topicNamespace + "/pose", &DriveToPoseControllerPrivate::OnCurrentPose, + this->dataPtr.get()); + + // Create velocity publisher + this->dataPtr->velocityPublisher = + this->dataPtr->node.Advertise(topicNamespace + "/cmd_vel"); + + // Create pose reached publisher + this->dataPtr->poseReachedPublisher = + this->dataPtr->node.Advertise(topicNamespace + "/reached_pose"); +} + +////////////////////////////////////////////////// +void DriveToPoseController::PostUpdate( + const UpdateInfo& _info, + const EntityComponentManager& /*_ecm*/) +{ + GZ_PROFILE("DriveToPoseController::PostUpdate"); + + if (_info.paused) return; + + // Calculate velocity and publish + this->dataPtr->CalculateVelocity(); +} + +////////////////////////////////////////////////// +void DriveToPoseControllerPrivate::CalculateVelocity() +{ + std::lock_guard lock(this->mutex); + + if (!this->targetPose || !this->currentPose) return; + + math::Pose3d target = *this->targetPose; + math::Pose3d current = *this->currentPose; + + double linearError = + sqrt(pow(target.X() - current.X(), 2) + pow(target.Y() - current.Y(), 2)); + + double angularError = target.Yaw() - current.Yaw(); + NormalizeAngle(angularError); + + if (linearError <= this->linearDeviation) + { + linearError = 0.0; + if (abs(angularError) <= this->angularDeviation) + { + angularError = 0.0; + this->targetPose.reset(); + } + } + else + { + double headingError = + atan2(target.Y() - current.Y(), target.X() - current.X()) - current.Yaw(); + NormalizeAngle(headingError); + if (abs(headingError) <= this->angularDeviation) + { + angularError = 0.0; + } + else + { + angularError = headingError; + linearError = 0.0; + } + } + + msgs::Twist cmdVelMsg; + cmdVelMsg.mutable_linear()->set_x(this->linearPGain * linearError); + cmdVelMsg.mutable_angular()->set_z(this->angularPGain * angularError); + this->velocityPublisher.Publish(cmdVelMsg); + + if (!this->targetPose) + this->poseReachedPublisher.Publish(msgs::Convert(*this->currentPose)); +} + +////////////////////////////////////////////////// +void DriveToPoseControllerPrivate::NormalizeAngle(double &_angle) +{ + if (_angle > GZ_PI) { _angle -= 2 * GZ_PI; } + else if (_angle < -GZ_PI) { _angle += 2 * GZ_PI; } +} + +////////////////////////////////////////////////// +void DriveToPoseControllerPrivate::OnCmdPose(const msgs::Pose_V &_msg) +{ + std::lock_guard lock(this->mutex); + this->targetPose = msgs::Convert(_msg.pose(0)); +} + +////////////////////////////////////////////////// +void DriveToPoseControllerPrivate::OnCurrentPose(const msgs::Pose_V &_msg) +{ + std::lock_guard lock(this->mutex); + this->currentPose = msgs::Convert(_msg.pose(0)); +} + +GZ_ADD_PLUGIN(DriveToPoseController, + System, + ISystemConfigure, + ISystemPostUpdate); + +GZ_ADD_PLUGIN_ALIAS(DriveToPoseController, + "gz::sim::systems::DriveToPoseController"); diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.hh b/src/systems/drive_to_pose_controller/DriveToPoseController.hh new file mode 100644 index 0000000000..69d0948c29 --- /dev/null +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.hh @@ -0,0 +1,67 @@ +#ifndef GZ_SIM_SYSTEMS_DRIVETOPOSECONTROLLER_HH_ +#define GZ_SIM_SYSTEMS_DRIVETOPOSECONTROLLER_HH_ + +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +namespace systems +{ + // Forward declaration + class DriveToPoseControllerPrivate; + + /// \brief DriveToPoseController is a simple proportional controller that + /// is attached to a model to move it in the y giving a pose in Gazebo's + /// world coordinate system. This is not a standalone plugin, and requires + /// the DiffDrive and OdometryPublisher plugins respectively. + /// + /// The plugin has the following tags: + /// + /// - ``: (Optional) Proportional gain for the linear velocity controller + /// Default: 1.0 + /// + /// - ``: (Optional) Proportional gain for the angular velocity controller + /// Default: 2.0 + /// + /// - ``: (Optional) Allowable linear deviation (in meters) from the desired coordinate + /// Default: 0.1 + /// + /// - ``: (Optional) Allowable angular deviation (in radians) from the desired orientation. + /// Default: 0.05 + class DriveToPoseController + : public System, + public ISystemConfigure, + public ISystemPostUpdate + { + /// \brief Constructor + public: DriveToPoseController(); + + /// \brief Destructor + public: ~DriveToPoseController() override = default; + + // Documentation inherited + public: void Configure( + const Entity& _entity, + const std::shared_ptr& _sdf, + EntityComponentManager& _ecm, + EventManager& _eventMgr) override; + + // Documentation inherited + public: void PostUpdate( + const UpdateInfo& _info, + const EntityComponentManager& _ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} +} +} +} + +#endif From c77c9c5e858e71c78e49438a0f73307291c7ff4e Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Tue, 12 Nov 2024 02:50:21 +0800 Subject: [PATCH 02/11] Added world for drive to pose controller and updated cmake Signed-off-by: Saurabh Kamat --- examples/worlds/drive_to_pose_controller.sdf | 379 +++++++++++++++++++ src/systems/CMakeLists.txt | 1 + 2 files changed, 380 insertions(+) create mode 100644 examples/worlds/drive_to_pose_controller.sdf diff --git a/examples/worlds/drive_to_pose_controller.sdf b/examples/worlds/drive_to_pose_controller.sdf new file mode 100644 index 0000000000..dabd152d49 --- /dev/null +++ b/examples/worlds/drive_to_pose_controller.sdf @@ -0,0 +1,379 @@ + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + 7 9 14 0 0.78 -2.35 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 1 + floating + + + + + + true + true + true + true + + + + + + docked + + + + + + + docked + + + + + 0 0 -9.8 + + 0.01 + 1 + 100 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfF_01 + + -5.795143 -0.956635 0 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_WallB_01 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfE_01 + + 4.73156 0.57943 0 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -4.827049 0 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfE_01 + + 4.73156 -8.6651 0 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -1.242668 0 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -3.038551 0 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ShelfD_01 + + 4.73156 -6.750542 0 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_GroundB_01 + + 0.0 0.0 -0.090092 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_Lamp_01 + + 0 0 -4 0 0 0 + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_Bucket_01 + + 0.433449 9.631706 0 0 0 -1.563161 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_Bucket_01 + + -1.8321 -6.3752 0 0 0 -1.563161 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_Bucket_01 + + 0.433449 8.59 0 0 0 -1.563161 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringA_01 + + 5.708138 8.616844 -0.017477 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringA_01 + + 3.408638 8.616844 -0.017477 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringA_01 + + -1.491287 5.222435 -0.017477 0 0 -1.583185 + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01 + + 3.324959 3.822449 -0.012064 0 0 1.563871 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01 + + 5.54171 3.816475 -0.015663 0 0 -1.583191 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01 + + 5.384239 6.137154 0 0 0 3.150000 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01 + + 3.236 6.137154 0 0 0 3.150000 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01 + + -1.573677 2.301994 -0.015663 0 0 -3.133191 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringC_01 + + -1.2196 9.407 -0.015663 0 0 1.563871 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_ClutteringD_01 + + -1.634682 -7.811813 -0.319559 0 0 0 + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/aws_robomaker_warehouse_TrashCanC_01 + + -1.592441 7.715420 0 0 0 0 + + + + + 0 0 8.5 0 0 0 + 0.5 0.5 0.5 1 + 0.2 0.2 0.2 1 + + 80 + 0.3 + 0.01 + 0.001 + + 1 + 0.1 0.1 -1 + + + + + 1.78 9.31 0.1 0 0 -1.57 + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/DeliveryBot + + + + + joint_tire_left + joint_tire_right + 0.52 + 0.06137 + 1 + -1 + 2 + -2 + 1.0 + -1.0 + 1 + -1 + + + + + + + + + 1.0 + 2.0 + 0.1 + 0.05 + + + + + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 1ec6aa8d7e..3d5bc34cff 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -112,6 +112,7 @@ add_subdirectory(contact) add_subdirectory(camera_video_recorder) add_subdirectory(detachable_joint) add_subdirectory(diff_drive) +add_subdirectory(drive_to_pose_controller) add_subdirectory(dvl) # elevator system causes compile erros on windows if (NOT WIN32) From c2d0d6164f9c1f4c81dfa0de83c1fe97b5d3d1a4 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Sat, 16 Nov 2024 23:34:11 +0800 Subject: [PATCH 03/11] Added test for drive_to_pose_controller Signed-off-by: Saurabh Kamat --- test/integration/CMakeLists.txt | 1 + .../drive_to_pose_controller_system.cc | 170 ++++++++++++++++++ test/worlds/drive_to_pose_controller.sdf | 84 +++++++++ 3 files changed, 255 insertions(+) create mode 100644 test/integration/drive_to_pose_controller_system.cc create mode 100644 test/worlds/drive_to_pose_controller.sdf diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index da422d2a89..a475e230e3 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -19,6 +19,7 @@ set(tests contact_system.cc detachable_joint.cc diff_drive_system.cc + drive_to_pose_controller_system.cc each_new_removed.cc entity_erase.cc entity_system.cc diff --git a/test/integration/drive_to_pose_controller_system.cc b/test/integration/drive_to_pose_controller_system.cc new file mode 100644 index 0000000000..c8db464d62 --- /dev/null +++ b/test/integration/drive_to_pose_controller_system.cc @@ -0,0 +1,170 @@ +#include + +#include +#include +#include + +#include +#include + +#include "gz/sim/Server.hh" + +#include "../helpers/EnvTestFixture.hh" +#include "../helpers/Relay.hh" + +using namespace gz; +using namespace sim; +using namespace std::chrono_literals; + +/// \brief Test DriveToPoseController system +class DriveToPoseControllerTest + : public InternalFixture<::testing::Test> +{ + /// \param[in] _sdfFile SDF file to load + /// \param[in] _topicPrefix Prefix for all topics + /// \param[in] _pose Pose command + protected: void TestPublishCmd(const std::string& _sdfFile, + const std::string& _topicPrefix, + const math::Pose3d& _pose) + { + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + const std::string cmdPoseTopic = _topicPrefix + "/cmd_pose"; + const std::string cmdVelTopic = _topicPrefix + "/cmd_vel"; + const std::string reachedPoseTopic = _topicPrefix + "/reached_pose"; + const std::string odomPoseTopic = _topicPrefix + "/pose"; + + // Get the reached pose message + math::Pose3d reachedPose; + std::function reachedPoseCb = + [&](const msgs::Pose &_msg) + { + reachedPose = msgs::Convert(_msg); + std::cout << "reached pose: " << reachedPose.X() << ", " + << reachedPose.Y() << std::endl; + }; + + // Get the odom pose message + math::Pose3d odomPose; + std::function odomPoseCb = + [&](const msgs::Pose_V &_msg) + { + odomPose = msgs::Convert(_msg.pose(0)); + }; + + // Count the last published linear and angular velocities and the total + // msg count + double lastLinearVel; + double lastAngularVel; + int msgCount = 0; + std::function cmdVelCb = + [&](const msgs::Twist &_msg) + { + ++msgCount; + lastLinearVel = _msg.linear().x(); + lastAngularVel = _msg.angular().y(); + }; + + transport::Node node; + auto pub = node.Advertise(cmdPoseTopic); + node.Subscribe(reachedPoseTopic, reachedPoseCb); + node.Subscribe(odomPoseTopic, odomPoseCb); + node.Subscribe(cmdVelTopic, cmdVelCb); + + // Publish a pose command + msgs::Pose_V poseMsg; + auto poseMsgPose = poseMsg.add_pose(); + poseMsgPose->CopyFrom(msgs::Convert(_pose)); + pub.Publish(poseMsg); + + // Run the system for 2s + test::Relay testSystem; + testSystem.OnPreUpdate( + [&](const UpdateInfo&, + const EntityComponentManager&){}); + server.AddSystem(testSystem.systemPtr); + server.Run(true, 2000, false); + + // Verify the final pose with the actual odom pose + ASSERT_NEAR(_pose.X(), odomPose.X(), 0.1); + ASSERT_NEAR(_pose.Y(), odomPose.Y(), 0.1); + ASSERT_NEAR(_pose.Yaw(), odomPose.Yaw(), 0.05); + + // Verify the final pose with the reached pose acknowledgement + ASSERT_NEAR(_pose.X(), reachedPose.X(), 0.1); + ASSERT_NEAR(_pose.Y(), reachedPose.Y(), 0.1); + ASSERT_NEAR(_pose.Yaw(), reachedPose.Yaw(), 0.05); + + // Verify the final velocity and check if the msg count is more than 1 + ASSERT_EQ(lastLinearVel, 0.0); + ASSERT_EQ(lastAngularVel, 0.0); + ASSERT_GT(msgCount, 0); + } +}; + +///////////////////////////////////////////////// +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", + "/model/DeliveryBot", + pose); +} + +///////////////////////////////////////////////// +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", + pose); +} + +///////////////////////////////////////////////// +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", + pose); +} + +///////////////////////////////////////////////// +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", + pose); +} + +///////////////////////////////////////////////// +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", + pose); +} diff --git a/test/worlds/drive_to_pose_controller.sdf b/test/worlds/drive_to_pose_controller.sdf new file mode 100644 index 0000000000..0ec2251c06 --- /dev/null +++ b/test/worlds/drive_to_pose_controller.sdf @@ -0,0 +1,84 @@ + + + + 0 0 -9.8 + + 0.01 + 0 + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + 0 0 0.1 0 0 0 + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/DeliveryBot + + + + + joint_tire_left + joint_tire_right + 0.52 + 0.06137 + 1 + -1 + 2 + -2 + 1.0 + -1.0 + 1 + -1 + + + + + + + + + 1.0 + 2.0 + 0.1 + 0.05 + + + + + From 59d4056b72de83de34c68f6e8d63976a549c039c Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Sat, 16 Nov 2024 23:34:41 +0800 Subject: [PATCH 04/11] Updated world name Signed-off-by: Saurabh Kamat --- examples/worlds/drive_to_pose_controller.sdf | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/examples/worlds/drive_to_pose_controller.sdf b/examples/worlds/drive_to_pose_controller.sdf index dabd152d49..e9342cd2d4 100644 --- a/examples/worlds/drive_to_pose_controller.sdf +++ b/examples/worlds/drive_to_pose_controller.sdf @@ -44,7 +44,7 @@ --> - + @@ -148,6 +148,11 @@ 1 100 + + + From 1892486d329c2a7605163a4a756dc4fd80d40864 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Sat, 16 Nov 2024 23:38:29 +0800 Subject: [PATCH 05/11] Added newline Signed-off-by: Saurabh Kamat --- src/systems/drive_to_pose_controller/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/drive_to_pose_controller/CMakeLists.txt b/src/systems/drive_to_pose_controller/CMakeLists.txt index a580005389..8840e3102d 100644 --- a/src/systems/drive_to_pose_controller/CMakeLists.txt +++ b/src/systems/drive_to_pose_controller/CMakeLists.txt @@ -3,4 +3,4 @@ gz_add_system(drive-to-pose-controller DriveToPoseController.cc PUBLIC_LINK_LIBS gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} -) \ No newline at end of file +) From bfdb4214a206622763906dc8de7684e59876758d Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Sat, 16 Nov 2024 23:47:44 +0800 Subject: [PATCH 06/11] Added copyright Signed-off-by: Saurabh Kamat --- .../DriveToPoseController.cc | 17 +++++++++++++++++ .../DriveToPoseController.hh | 17 +++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.cc b/src/systems/drive_to_pose_controller/DriveToPoseController.cc index 1daed8e2a0..ea801de395 100644 --- a/src/systems/drive_to_pose_controller/DriveToPoseController.cc +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.cc @@ -1,3 +1,20 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + #include "DriveToPoseController.hh" #include diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.hh b/src/systems/drive_to_pose_controller/DriveToPoseController.hh index 69d0948c29..2c43f2f6ea 100644 --- a/src/systems/drive_to_pose_controller/DriveToPoseController.hh +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.hh @@ -1,3 +1,20 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + #ifndef GZ_SIM_SYSTEMS_DRIVETOPOSECONTROLLER_HH_ #define GZ_SIM_SYSTEMS_DRIVETOPOSECONTROLLER_HH_ From 894543b6f618c74fcf2e8074bab1b2da3ac92028 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Sun, 17 Nov 2024 00:10:53 +0800 Subject: [PATCH 07/11] Styling changes Signed-off-by: Saurabh Kamat --- .../DriveToPoseController.cc | 10 ++++++++-- .../DriveToPoseController.hh | 18 ++++++++++-------- .../drive_to_pose_controller_system.cc | 17 +++++++++++++++++ 3 files changed, 35 insertions(+), 10 deletions(-) diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.cc b/src/systems/drive_to_pose_controller/DriveToPoseController.cc index ea801de395..af7d020f03 100644 --- a/src/systems/drive_to_pose_controller/DriveToPoseController.cc +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.cc @@ -219,8 +219,14 @@ void DriveToPoseControllerPrivate::CalculateVelocity() ////////////////////////////////////////////////// void DriveToPoseControllerPrivate::NormalizeAngle(double &_angle) { - if (_angle > GZ_PI) { _angle -= 2 * GZ_PI; } - else if (_angle < -GZ_PI) { _angle += 2 * GZ_PI; } + if (_angle > GZ_PI) + { + _angle -= 2 * GZ_PI; + } + else if (_angle < -GZ_PI) + { + _angle += 2 * GZ_PI; + } } ////////////////////////////////////////////////// diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.hh b/src/systems/drive_to_pose_controller/DriveToPoseController.hh index 2c43f2f6ea..3fdd063dc0 100644 --- a/src/systems/drive_to_pose_controller/DriveToPoseController.hh +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.hh @@ -18,6 +18,8 @@ #ifndef GZ_SIM_SYSTEMS_DRIVETOPOSECONTROLLER_HH_ #define GZ_SIM_SYSTEMS_DRIVETOPOSECONTROLLER_HH_ +#include + #include namespace gz @@ -39,17 +41,17 @@ namespace systems /// /// The plugin has the following tags: /// - /// - ``: (Optional) Proportional gain for the linear velocity controller - /// Default: 1.0 + /// - ``: (Optional) Proportional gain for the linear + /// velocity controller | Default: 1.0 /// - /// - ``: (Optional) Proportional gain for the angular velocity controller - /// Default: 2.0 + /// - ``: (Optional) Proportional gain for the angular + /// velocity controller | Default: 2.0 /// - /// - ``: (Optional) Allowable linear deviation (in meters) from the desired coordinate - /// Default: 0.1 + /// - ``: (Optional) Allowable linear deviation (in meters) + /// from the desired coordinate | Default: 0.1 /// - /// - ``: (Optional) Allowable angular deviation (in radians) from the desired orientation. - /// Default: 0.05 + /// - ``: (Optional) Allowable angular deviation (in rad) + /// from the desired orientation | Default: 0.05 class DriveToPoseController : public System, public ISystemConfigure, diff --git a/test/integration/drive_to_pose_controller_system.cc b/test/integration/drive_to_pose_controller_system.cc index c8db464d62..f5fad347b9 100644 --- a/test/integration/drive_to_pose_controller_system.cc +++ b/test/integration/drive_to_pose_controller_system.cc @@ -1,3 +1,20 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + #include #include From 8478e28c5393fd73e12049c3a2c2ee31053ed6e7 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Sun, 17 Nov 2024 01:27:08 +0800 Subject: [PATCH 08/11] Missing include for lint Signed-off-by: Saurabh Kamat --- src/systems/drive_to_pose_controller/DriveToPoseController.hh | 1 + 1 file changed, 1 insertion(+) diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.hh b/src/systems/drive_to_pose_controller/DriveToPoseController.hh index 3fdd063dc0..a30eb51bda 100644 --- a/src/systems/drive_to_pose_controller/DriveToPoseController.hh +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.hh @@ -19,6 +19,7 @@ #define GZ_SIM_SYSTEMS_DRIVETOPOSECONTROLLER_HH_ #include +#include #include From 3f28d907f0503bc060233f103c2f46a466d048c8 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Sun, 17 Nov 2024 17:45:26 +0800 Subject: [PATCH 09/11] Fixed typo in copyright Signed-off-by: Saurabh Kamat --- src/systems/drive_to_pose_controller/DriveToPoseController.cc | 2 +- src/systems/drive_to_pose_controller/DriveToPoseController.hh | 2 +- test/integration/drive_to_pose_controller_system.cc | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.cc b/src/systems/drive_to_pose_controller/DriveToPoseController.cc index af7d020f03..3a25201dac 100644 --- a/src/systems/drive_to_pose_controller/DriveToPoseController.cc +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2023 Open Source Robotics Foundation + * Copyright (C) 2024 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.hh b/src/systems/drive_to_pose_controller/DriveToPoseController.hh index a30eb51bda..fa42d6f536 100644 --- a/src/systems/drive_to_pose_controller/DriveToPoseController.hh +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2023 Open Source Robotics Foundation + * Copyright (C) 2024 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. diff --git a/test/integration/drive_to_pose_controller_system.cc b/test/integration/drive_to_pose_controller_system.cc index f5fad347b9..ec5691adae 100644 --- a/test/integration/drive_to_pose_controller_system.cc +++ b/test/integration/drive_to_pose_controller_system.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2023 Open Source Robotics Foundation + * Copyright (C) 2024 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. From c00bbdde999372b38a5fb51b1e142395cfe837ac Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Thu, 21 Nov 2024 19:10:37 +0800 Subject: [PATCH 10/11] Added warnings when correct set of plugins are not loaded Signed-off-by: Saurabh Kamat --- .../DriveToPoseController.cc | 36 +++++++++++++++-- .../DriveToPoseController.hh | 10 +++-- .../drive_to_pose_controller_system.cc | 39 +++++++++++-------- 3 files changed, 63 insertions(+), 22 deletions(-) diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.cc b/src/systems/drive_to_pose_controller/DriveToPoseController.cc index 3a25201dac..70ad26ddc6 100644 --- a/src/systems/drive_to_pose_controller/DriveToPoseController.cc +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.cc @@ -30,6 +30,11 @@ #include #include +#include +#include +#include +#include + 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 publishers; + std::vector 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(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(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; } ////////////////////////////////////////////////// diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.hh b/src/systems/drive_to_pose_controller/DriveToPoseController.hh index fa42d6f536..932e917bc2 100644 --- a/src/systems/drive_to_pose_controller/DriveToPoseController.hh +++ b/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 -#include - +#include +#include +#include #include +#include + +#include + namespace gz { namespace sim diff --git a/test/integration/drive_to_pose_controller_system.cc b/test/integration/drive_to_pose_controller_system.cc index ec5691adae..60b70d9eaa 100644 --- a/test/integration/drive_to_pose_controller_system.cc +++ b/test/integration/drive_to_pose_controller_system.cc @@ -24,6 +24,10 @@ #include #include +#include +#include +#include + #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); } From c10acbde173c0f12fbe48fab4ab3afd16856df15 Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Thu, 21 Nov 2024 20:17:24 +0800 Subject: [PATCH 11/11] Fixed styling errors Signed-off-by: Saurabh Kamat --- .../DriveToPoseController.cc | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/systems/drive_to_pose_controller/DriveToPoseController.cc b/src/systems/drive_to_pose_controller/DriveToPoseController.cc index 70ad26ddc6..e2d78424e8 100644 --- a/src/systems/drive_to_pose_controller/DriveToPoseController.cc +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.cc @@ -34,6 +34,7 @@ #include #include #include +#include using namespace gz; using namespace sim; @@ -176,11 +177,16 @@ void DriveToPoseController::Configure( this->dataPtr->poseReachedPublisher = this->dataPtr->node.Advertise(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; + 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; } //////////////////////////////////////////////////