diff --git a/examples/worlds/drive_to_pose_controller.sdf b/examples/worlds/drive_to_pose_controller.sdf new file mode 100644 index 0000000000..8da20faf98 --- /dev/null +++ b/examples/worlds/drive_to_pose_controller.sdf @@ -0,0 +1,380 @@ + + + + + + + + + 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) 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..8840e3102d --- /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} +) 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..e2d78424e8 --- /dev/null +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.cc @@ -0,0 +1,288 @@ +/* + * 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. + * 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 + +#include +#include +#include +#include + +#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 odometry pose publisher + this->dataPtr->node.Subscribe( + 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 + "/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; +} + +////////////////////////////////////////////////// +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..b61d62ed47 --- /dev/null +++ b/src/systems/drive_to_pose_controller/DriveToPoseController.hh @@ -0,0 +1,91 @@ +/* + * 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. + * 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_ + +#include +#include +#include +#include + +#include + +#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 by 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 rad) + /// 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 diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index df28287316..6c2f43cd98 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..831bc148e7 --- /dev/null +++ b/test/integration/drive_to_pose_controller_system.cc @@ -0,0 +1,195 @@ +/* + * 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. + * 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 +#include +#include + +#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); + }; + + // 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( + gz::common::joinPaths( + 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( + gz::common::joinPaths( + 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( + gz::common::joinPaths( + 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( + gz::common::joinPaths( + 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( + gz::common::joinPaths( + 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 + + + + +