diff --git a/examples/worlds/velocity_control.sdf b/examples/worlds/velocity_control.sdf new file mode 100644 index 0000000000..d010b205ef --- /dev/null +++ b/examples/worlds/velocity_control.sdf @@ -0,0 +1,492 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/velocity_control/control + /world/velocity_control/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/velocity_control/stats + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 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 2 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + + + + + + 0 -2 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 1.0 0.5 1 + 0.5 1.0 0.5 1 + 0.0 1.0 0.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + + + + + + diff --git a/include/ignition/gazebo/components/AngularVelocityCmd.hh b/include/ignition/gazebo/components/AngularVelocityCmd.hh new file mode 100644 index 0000000000..77edfd277a --- /dev/null +++ b/include/ignition/gazebo/components/AngularVelocityCmd.hh @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2020 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 IGNITION_GAZEBO_COMPONENTS_ANGULARVELOCITYCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_ANGULARVELOCITYCMD_HH_ + +#include + +#include + +#include +#include "ignition/gazebo/components/Component.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains angular velocity cmd of an entity + /// represented by ignition::math::Vector3d. + using AngularVelocityCmd = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.AngularVelocityCmd", AngularVelocityCmd) + + /// \brief A component type that contains angular velocity cmd + /// of an entity in the world frame represented by ignition::math::Vector3d. + using WorldAngularVelocityCmd = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.WorldAngularVelocityCmd", WorldAngularVelocityCmd) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/LinearVelocityCmd.hh b/include/ignition/gazebo/components/LinearVelocityCmd.hh new file mode 100644 index 0000000000..e68eac2eeb --- /dev/null +++ b/include/ignition/gazebo/components/LinearVelocityCmd.hh @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2020 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 IGNITION_GAZEBO_COMPONENTS_LINEARVELOCITYCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_LINEARVELOCITYCMD_HH_ + +#include + +#include + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains linear velocity of an entity + /// represented by ignition::math::Vector3d. + using LinearVelocityCmd = Component< + math::Vector3d, class LinearVelocityCmdTag>; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.LinearVelocityCmd", LinearVelocityCmd) + + /// \brief A component type that contains linear velocity of an entity in the + /// world frame represented by ignition::math::Vector3d. + using WorldLinearVelocityCmd = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.WorldLinearVelocityCmd", WorldLinearVelocityCmd) +} +} +} +} + +#endif diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 6c8bef5e41..37c90ffc64 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -790,7 +790,6 @@ void IgnRenderer::HandleKeyPress(QKeyEvent *_e) } } - switch (_e->key()) { case Qt::Key_X: @@ -1344,7 +1343,10 @@ void IgnRenderer::HandleMouseViewControl() // Pan with left button if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::LEFT) { - this->dataPtr->viewControl.Pan(this->dataPtr->drag); + if (Qt::ShiftModifier == QGuiApplication::queryKeyboardModifiers()) + this->dataPtr->viewControl.Orbit(this->dataPtr->drag); + else + this->dataPtr->viewControl.Pan(this->dataPtr->drag); } // Orbit with middle button else if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::MIDDLE) diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 72626deeee..3abdd8e713 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -97,4 +97,5 @@ add_subdirectory(thermal) add_subdirectory(touch_plugin) add_subdirectory(triggered_publisher) add_subdirectory(user_commands) +add_subdirectory(velocity_control) add_subdirectory(wind_effects) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 3b7cb1caf6..3b22dad504 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -75,6 +76,7 @@ // Components #include "ignition/gazebo/components/AngularAcceleration.hh" #include "ignition/gazebo/components/AngularVelocity.hh" +#include "ignition/gazebo/components/AngularVelocityCmd.hh" #include "ignition/gazebo/components/AxisAlignedBox.hh" #include "ignition/gazebo/components/BatterySoC.hh" #include "ignition/gazebo/components/CanonicalLink.hh" @@ -95,6 +97,7 @@ #include "ignition/gazebo/components/JointVelocityReset.hh" #include "ignition/gazebo/components/LinearAcceleration.hh" #include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/LinearVelocityCmd.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" @@ -421,6 +424,25 @@ class ignition::gazebo::systems::PhysicsPrivate public: std::unordered_map entityJointVelocityCommandMap; + ////////////////////////////////////////////////// + // World velocity command + public: using WorldVelocityCommandFeatureList = + ignition::physics::FeatureList< + ignition::physics::SetFreeGroupWorldVelocity>; + + /// \brief Free group type with world velocity command. + public: using FreeGroupVelocityCmdPtrType = + ignition::physics::FreeGroupPtr< + ignition::physics::FeaturePolicy3d, + WorldVelocityCommandFeatureList>; + + /// \brief A map between free group entity ids in the ECM + /// to FreeGroup Entities in ign-physics, with velocity command feature. + /// All FreeGroup on this map are casted for + /// `WorldVelocityCommandFeatureList`. + public: std::unordered_map + entityWorldVelocityCommandMap; + ////////////////////////////////////////////////// // Meshes @@ -1316,6 +1338,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; }); + // Update model pose _ecm.Each( [&](const Entity &_entity, const components::Model *, const components::WorldPoseCmd *_poseCmd) @@ -1364,6 +1387,68 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; }); + // Update model angular velocity + _ecm.Each( + [&](const Entity &_entity, const components::Model *, + const components::AngularVelocityCmd *_angularVelocityCmd) + { + auto modelIt = this->entityModelMap.find(_entity); + if (modelIt == this->entityModelMap.end()) + return true; + + auto freeGroup = modelIt->second->FindFreeGroup(); + if (!freeGroup) + return true; + + const components::Pose *poseComp = + _ecm.Component(_entity); + math::Vector3d worldAngularVel = poseComp->Data().Rot() * + _angularVelocityCmd->Data(); + + auto worldAngularVelFeature = entityCast(_entity, freeGroup, + this->entityWorldVelocityCommandMap); + if (!worldAngularVelFeature) + { + return true; + } + + worldAngularVelFeature->SetWorldAngularVelocity( + math::eigen3::convert(worldAngularVel)); + + return true; + }); + + // Update model linear velocity + _ecm.Each( + [&](const Entity &_entity, const components::Model *, + const components::LinearVelocityCmd *_linearVelocityCmd) + { + auto modelIt = this->entityModelMap.find(_entity); + if (modelIt == this->entityModelMap.end()) + return true; + + auto freeGroup = modelIt->second->FindFreeGroup(); + if (!freeGroup) + return true; + + const components::Pose *poseComp = + _ecm.Component(_entity); + math::Vector3d worldLinearVel = poseComp->Data().Rot() * + _linearVelocityCmd->Data(); + + auto worldLinearVelFeature = entityCast(_entity, freeGroup, + this->entityWorldVelocityCommandMap); + if (!worldLinearVelFeature) + { + return true; + } + + worldLinearVelFeature->SetWorldLinearVelocity( + math::eigen3::convert(worldLinearVel)); + + return true; + }); + // Clear pending commands // Note: Removing components from inside an Each call can be dangerous. // Instead, we collect all the entities that have the desired components and diff --git a/src/systems/velocity_control/CMakeLists.txt b/src/systems/velocity_control/CMakeLists.txt new file mode 100644 index 0000000000..a2c905b677 --- /dev/null +++ b/src/systems/velocity_control/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(velocity-control + SOURCES + VelocityControl.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc new file mode 100644 index 0000000000..b431b86c7d --- /dev/null +++ b/src/systems/velocity_control/VelocityControl.cc @@ -0,0 +1,200 @@ +/* + * Copyright (C) 2020 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 "ignition/gazebo/components/AngularVelocityCmd.hh" +#include "ignition/gazebo/components/LinearVelocityCmd.hh" +#include "ignition/gazebo/Model.hh" + +#include "VelocityControl.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::VelocityControlPrivate +{ + /// \brief Callback for velocity subscription + /// \param[in] _msg Velocity message + public: void OnCmdVel(const ignition::msgs::Twist &_msg); + + /// \brief Update the linear and angular velocities. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief Angular velocity of a model + public: math::Vector3d angularVelocity{0, 0, 0}; + + /// \brief Linear velocity of a model + public: math::Vector3d linearVelocity{0, 0, 0}; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief Last target velocity requested. + public: msgs::Twist targetVel; + + /// \brief A mutex to protect the target velocity command. + public: std::mutex mutex; +}; + +////////////////////////////////////////////////// +VelocityControl::VelocityControl() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void VelocityControl::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "VelocityControl plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + // Subscribe to commands + std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; + if (_sdf->HasElement("topic")) + topic = _sdf->Get("topic"); + this->dataPtr->node.Subscribe( + topic, &VelocityControlPrivate::OnCmdVel, this->dataPtr.get()); + + ignmsg << "VelocityControl subscribing to twist messages on [" << topic << "]" + << std::endl; +} + +////////////////////////////////////////////////// +void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("VelocityControl::PreUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + // Nothing left to do if paused. + if (_info.paused) + return; + + // update angular velocity of model + auto angularVel = + _ecm.Component( + this->dataPtr->model.Entity()); + + if (angularVel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->model.Entity(), + components::AngularVelocityCmd({this->dataPtr->angularVelocity})); + } + else + { + *angularVel = + components::AngularVelocityCmd({this->dataPtr->angularVelocity}); + } + + // update linear velocity of model + auto linearVel = + _ecm.Component( + this->dataPtr->model.Entity()); + + if (linearVel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->model.Entity(), + components::LinearVelocityCmd({this->dataPtr->linearVelocity})); + } + else + { + *linearVel = + components::LinearVelocityCmd({this->dataPtr->linearVelocity}); + } +} + +////////////////////////////////////////////////// +void VelocityControl::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("VelocityControl::PostUpdate"); + // Nothing left to do if paused. + if (_info.paused) + return; + + this->dataPtr->UpdateVelocity(_info, _ecm); +} + + +////////////////////////////////////////////////// +void VelocityControlPrivate::UpdateVelocity( + const ignition::gazebo::UpdateInfo &/*_info*/, + const ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ + IGN_PROFILE("VeocityControl::UpdateVelocity"); + + double linVel; + double angVel; + { + std::lock_guard lock(this->mutex); + linVel = this->targetVel.linear().x(); + angVel = this->targetVel.angular().z(); + } + + this->linearVelocity = math::Vector3d( + linVel, this->targetVel.linear().y(), this->targetVel.linear().z()); + this->angularVelocity = math::Vector3d( + this->targetVel.angular().x(), this->targetVel.angular().y(), angVel); +} + +////////////////////////////////////////////////// +void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg) +{ + std::lock_guard lock(this->mutex); + this->targetVel = _msg; +} + +IGNITION_ADD_PLUGIN(VelocityControl, + ignition::gazebo::System, + VelocityControl::ISystemConfigure, + VelocityControl::ISystemPreUpdate, + VelocityControl::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(VelocityControl, + "ignition::gazebo::systems::VelocityControl") diff --git a/src/systems/velocity_control/VelocityControl.hh b/src/systems/velocity_control/VelocityControl.hh new file mode 100644 index 0000000000..a4427f22c2 --- /dev/null +++ b/src/systems/velocity_control/VelocityControl.hh @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2020 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 IGNITION_GAZEBO_SYSTEMS_VELOCITYCONTROL_HH_ +#define IGNITION_GAZEBO_SYSTEMS_VELOCITYCONTROL_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class VelocityControlPrivate; + + /// \brief Linear and angular velocity controller + /// which is directly set on a model. + class IGNITION_GAZEBO_VISIBLE VelocityControl + : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: VelocityControl(); + + /// \brief Destructor + public: ~VelocityControl() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &/*_info*/, + ignition::gazebo::EntityComponentManager &_ecm) 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 bdbe1850e9..6468663472 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -37,6 +37,7 @@ set(tests touch_plugin.cc triggered_publisher.cc user_commands.cc + velocity_control_system.cc log_system.cc wind_effects.cc ) diff --git a/test/integration/velocity_control_system.cc b/test/integration/velocity_control_system.cc new file mode 100644 index 0000000000..b3f21ef9dd --- /dev/null +++ b/test/integration/velocity_control_system.cc @@ -0,0 +1,150 @@ +/* + * Copyright (C) 2020 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 "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test VelocityControl system +class VelocityControlTest : public ::testing::TestWithParam +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + protected: void TestPublishCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle_blue")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(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 + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + + msgs::Twist msg; + + // Avoid wheel slip by limiting acceleration (1 m/s^2) + // and max velocity (0.5 m/s). + // See parameters + // in "/test/worlds/velocity_control.sdf". + test::Relay velocityRamp; + const double desiredLinVel = 10.5; + const double desiredAngVel = 0.2; + velocityRamp.OnPreUpdate( + [&](const gazebo::UpdateInfo &/*_info*/, + const gazebo::EntityComponentManager &) + { + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, desiredAngVel)); + pub.Publish(msg); + }); + + server.AddSystem(velocityRamp.systemPtr); + + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + + ASSERT_NE(maxSleep, sleep); + + // verify that the vehicle is moving in +x and rotating towards +y + for (unsigned int i = 1001; i < poses.size(); ++i) + { + EXPECT_GT(poses[i].Pos().X(), poses[i-1].Pos().X()); + EXPECT_GT(poses[i].Pos().Y(), poses[i-1].Pos().Y()); + EXPECT_NEAR(poses[i].Pos().Z(), poses[i-1].Pos().Z(), 1e-5); + EXPECT_NEAR(poses[i].Rot().Euler().X(), + poses[i-1].Rot().Euler().X(), 1e-5); + EXPECT_NEAR(poses[i].Rot().Euler().Y(), + poses[i-1].Rot().Euler().Y(), 1e-5); + EXPECT_GT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z()); + } + } +}; + +///////////////////////////////////////////////// +TEST_P(VelocityControlTest, PublishCmd) +{ + TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/velocity_control.sdf", + "/model/vehicle_blue/cmd_vel"); +} + +// Run multiple times +INSTANTIATE_TEST_CASE_P(ServerRepeat, VelocityControlTest, + ::testing::Range(1, 2)); diff --git a/test/worlds/velocity_control.sdf b/test/worlds/velocity_control.sdf new file mode 100644 index 0000000000..d010b205ef --- /dev/null +++ b/test/worlds/velocity_control.sdf @@ -0,0 +1,492 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/velocity_control/control + /world/velocity_control/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/velocity_control/stats + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 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 2 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + + + + + + 0 -2 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 1.0 0.5 1 + 0.5 1.0 0.5 1 + 0.0 1.0 0.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + + + + + +