From 33b37c5dab495c733f5a0beda7f53872bb7776e9 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Wed, 14 Apr 2021 16:31:21 -0400 Subject: [PATCH 1/6] Update benchmark comparison instructions (#766) (#766) Signed-off-by: Ashton Larkin --- test/benchmark/README.md | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/test/benchmark/README.md b/test/benchmark/README.md index 2fb4934c99..a5f7f79b8a 100644 --- a/test/benchmark/README.md +++ b/test/benchmark/README.md @@ -14,24 +14,24 @@ Given a set of changes to the codebase, it is often useful to see the difference in performance. -Once two (or more) benchmarks have been executed, compare the results with: +Once two (or more) benchmarks have been executed, compare the results by downloading the [benchmark tools](https://github.com/google/benchmark/tree/master/tools), and then run the following command: - ``` - # From the build folder - ../test/gbenchmark/tools/compare.py benchmarks baseline.json contender.json - ``` +``` +# From the downloaded "tools" folder +./compare.py benchmarks baseline.json contender.json +``` ### CPU Scaling Warnings Note: If you receive warnings about CPU scaling, you can change the CPU governor with: - ``` - sudo cpupower frequency-set --governor performance - ``` +``` +sudo cpupower frequency-set --governor performance +``` The previous setting can be restored with: - ``` - sudo cpupower frequency-set --governor powersave - ``` +``` +sudo cpupower frequency-set --governor powersave +``` From c46c69b47574e6ffd8c924d4c90f237b0db07a83 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 23 Apr 2021 17:00:44 -0700 Subject: [PATCH 2/6] =?UTF-8?q?=F0=9F=91=A9=E2=80=8D=F0=9F=8C=BE=20Enable?= =?UTF-8?q?=20Focal=20CI=20(#646)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel Co-authored-by: Michael Carroll --- .github/workflows/ci.yml | 19 +++++++++---------- src/ServerPrivate.cc | 2 +- src/SimulationRunner.cc | 6 +++--- test/integration/diff_drive_system.cc | 6 +++++- test/integration/wheel_slip.cc | 6 ++++++ 5 files changed, 24 insertions(+), 15 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index cf70ca6852..705db4f35d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -14,13 +14,12 @@ jobs: uses: ignition-tooling/action-ignition-ci@bionic with: codecov-token: ${{ secrets.CODECOV_TOKEN }} - # TODO(anyone) Enable Focal CI and fix failing tests - # focal-ci: - # runs-on: ubuntu-latest - # name: Ubuntu Focal CI - # steps: - # - name: Checkout - # uses: actions/checkout@v2 - # - name: Compile and test - # id: ci - # uses: ignition-tooling/action-ignition-ci@focal + focal-ci: + runs-on: ubuntu-latest + name: Ubuntu Focal CI + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Compile and test + id: ci + uses: ignition-tooling/action-ignition-ci@focal diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 67fa6df18c..22dc0848dd 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -311,7 +311,7 @@ void ServerPrivate::AddRecordPlugin(const ServerConfig &_config) // In the case that the --compress flag is set, then // this field will be populated with just the file extension - if(_config.LogRecordCompressPath() == ".zip") + if (_config.LogRecordCompressPath() == ".zip") { sdfCompressPath = std::string(sdfRecordPath); if (!std::string(1, sdfCompressPath.back()).compare( diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 79e4a1f34e..88de9d1d49 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -864,18 +864,18 @@ void SimulationRunner::LoadLoggingPlugins(const ServerConfig &_config) { std::list plugins; - if(_config.UseLogRecord() && !_config.LogPlaybackPath().empty()) + if (_config.UseLogRecord() && !_config.LogPlaybackPath().empty()) { ignwarn << "Both recording and playback are specified, defaulting to playback\n"; } - if(!_config.LogPlaybackPath().empty()) + if (!_config.LogPlaybackPath().empty()) { auto playbackPlugin = _config.LogPlaybackPlugin(); plugins.push_back(playbackPlugin); } - else if(_config.UseLogRecord()) + else if (_config.UseLogRecord()) { auto recordPlugin = _config.LogRecordPlugin(); plugins.push_back(recordPlugin); diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 373eaf745e..df756fcd52 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -304,11 +304,15 @@ TEST_P(DiffDriveTest, SkidPublishCmd) EXPECT_EQ(3u, odomPoses.size()); EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X()); - EXPECT_LT(poses[0].Pos().Y(), poses[3999].Pos().Y()); EXPECT_NEAR(poses[0].Pos().Z(), poses[3999].Pos().Z(), tol); EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); + + // Slip works on DART>=6.10, which isn't available on Ubuntu Focal +#ifndef __linux__ + EXPECT_LT(poses[0].Pos().Y(), poses[3999].Pos().Y()); EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); +#endif } ///////////////////////////////////////////////// diff --git a/test/integration/wheel_slip.cc b/test/integration/wheel_slip.cc index 7480197a35..ebc26042fa 100644 --- a/test/integration/wheel_slip.cc +++ b/test/integration/wheel_slip.cc @@ -554,12 +554,15 @@ TEST_F(WheelSlipTest, TricyclesUphill) server.AddSystem(testSlipSystem.systemPtr); server.Run(true, 2000, false); + // Slip works on DART>=6.10, which isn't available on Ubuntu Focal +#ifndef __linux__ // compute expected slip // normal force as passed to Wheel Slip in test world const double wheelNormalForce = 32; const double mass = 14.5; const double forceRatio = (mass/2) * std::abs(gravity->Data().X()) / wheelNormalForce; +#endif const double noSlipLinearSpeed = wheelRadius * angularSpeed; auto wheelRearLeftVelocity = @@ -590,6 +593,9 @@ TEST_F(WheelSlipTest, TricyclesUphill) EXPECT_NEAR(angularSpeed, wheelRearLeftVelocity->Data()[0], 3e-3); EXPECT_NEAR(angularSpeed, wheelRearRightVelocity->Data()[0], 3e-3); + // Slip works on DART>=6.10, which isn't available on Ubuntu Focal +#ifndef __linux__ EXPECT_NEAR(noSlipLinearSpeed - worldVelTrisphere1->Data()[0], noSlipLinearSpeed * forceRatio, 5e-3); +#endif } From a5f80ccdf3876c446bdc3c9224b5afcbdfb2263e Mon Sep 17 00:00:00 2001 From: Claire Wang <22240514+claireyywang@users.noreply.github.com> Date: Fri, 23 Apr 2021 17:01:52 -0700 Subject: [PATCH 3/6] [TPE] Support setting individual link velocity (#427) Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> Signed-off-by: Ian Chen Co-authored-by: Ian Chen Co-authored-by: Louise Poubel --- examples/worlds/pendulum_links.sdf | 279 ++++++++++++++++++ src/systems/physics/Physics.cc | 119 ++++++++ .../velocity_control/VelocityControl.cc | 217 ++++++++++++-- test/integration/velocity_control_system.cc | 115 ++++++++ test/worlds/velocity_control.sdf | 1 + 5 files changed, 703 insertions(+), 28 deletions(-) create mode 100644 examples/worlds/pendulum_links.sdf diff --git a/examples/worlds/pendulum_links.sdf b/examples/worlds/pendulum_links.sdf new file mode 100644 index 0000000000..2f62bab25c --- /dev/null +++ b/examples/worlds/pendulum_links.sdf @@ -0,0 +1,279 @@ + + + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 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 + + + + + + + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + + + + base + upper_link + lower_link + + + + diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 672d9bb828..72ccc67339 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1563,6 +1563,111 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; }); + // Update link angular velocity + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + const components::AngularVelocityCmd *_angularVelocityCmd) + { + if (!this->entityLinkMap.HasEntity(_entity)) + { + ignwarn << "Failed to find link [" << _entity + << "]." << std::endl; + return true; + } + + auto linkPtrPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPtrPhys) + return true; + + auto freeGroup = linkPtrPhys->FindFreeGroup(); + if (!freeGroup) + return true; + this->entityFreeGroupMap.AddEntity(_entity, freeGroup); + + auto worldAngularVelFeature = + this->entityFreeGroupMap + .EntityCast(_entity); + + if (!worldAngularVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set link angular velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + // velocity in world frame = world_to_model_tf * model_to_link_tf * vel + Entity modelEntity = topLevelModel(_entity, _ecm); + const components::Pose *modelEntityPoseComp = + _ecm.Component(modelEntity); + math::Pose3d modelToLinkTransform = this->RelativePose( + modelEntity, _entity, _ecm); + math::Vector3d worldAngularVel = modelEntityPoseComp->Data().Rot() + * modelToLinkTransform.Rot() * _angularVelocityCmd->Data(); + worldAngularVelFeature->SetWorldAngularVelocity( + math::eigen3::convert(worldAngularVel)); + + return true; + }); + + // Update link linear velocity + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + const components::LinearVelocityCmd *_linearVelocityCmd) + { + if (!this->entityLinkMap.HasEntity(_entity)) + { + ignwarn << "Failed to find link [" << _entity + << "]." << std::endl; + return true; + } + + auto linkPtrPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPtrPhys) + return true; + + auto freeGroup = linkPtrPhys->FindFreeGroup(); + if (!freeGroup) + return true; + this->entityFreeGroupMap.AddEntity(_entity, freeGroup); + + auto worldLinearVelFeature = + this->entityFreeGroupMap + .EntityCast(_entity); + if (!worldLinearVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set link linear velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + + // velocity in world frame = world_to_model_tf * model_to_link_tf * vel + Entity modelEntity = topLevelModel(_entity, _ecm); + const components::Pose *modelEntityPoseComp = + _ecm.Component(modelEntity); + math::Pose3d modelToLinkTransform = this->RelativePose( + modelEntity, _entity, _ecm); + math::Vector3d worldLinearVel = modelEntityPoseComp->Data().Rot() + * modelToLinkTransform.Rot() * _linearVelocityCmd->Data(); + worldLinearVelFeature->SetWorldLinearVelocity( + math::eigen3::convert(worldLinearVel)); + + return true; + }); + + // Populate bounding box info // Only compute bounding box if component exists to avoid unnecessary // computations @@ -2027,6 +2132,20 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) return true; }); + _ecm.Each( + [&](const Entity &, components::AngularVelocityCmd *_vel) -> bool + { + _vel->Data() = math::Vector3d::Zero; + return true; + }); + + _ecm.Each( + [&](const Entity &, components::LinearVelocityCmd *_vel) -> bool + { + _vel->Data() = math::Vector3d::Zero; + return true; + }); + // Update joint positions _ecm.Each( [&](const Entity &_entity, components::Joint *, diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index d6e07f48e7..7ed361c627 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -15,8 +15,11 @@ * */ +#include #include #include +#include +#include #include #include @@ -35,10 +38,15 @@ using namespace systems; class ignition::gazebo::systems::VelocityControlPrivate { - /// \brief Callback for velocity subscription + /// \brief Callback for model velocity subscription /// \param[in] _msg Velocity message public: void OnCmdVel(const ignition::msgs::Twist &_msg); + /// \brief Callback for link velocity subscription + /// \param[in] _msg Velocity message + public: void OnLinkCmdVel(const ignition::msgs::Twist &_msg, + const ignition::transport::MessageInfo &_info); + /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation @@ -46,23 +54,45 @@ class ignition::gazebo::systems::VelocityControlPrivate public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm); + /// \brief Update link velocity. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateLinkVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + /// \brief Ignition communication node. public: transport::Node node; + /// \brief Model interface + public: Model model{kNullEntity}; + /// \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. + /// \brief A mutex to protect the model velocity command. public: std::mutex mutex; + + /// \brief Link names + public: std::vector linkNames; + + /// \brief Link entities in a model + public: std::unordered_map links; + + /// \brief Angular velocities of links + public: std::unordered_map angularVelocities; + + /// \brief Linear velocities of links + public: std::unordered_map linearVelocities; + + /// \brief All link velocites + public: std::unordered_map linkVels; }; ////////////////////////////////////////////////// @@ -86,15 +116,45 @@ void VelocityControl::Configure(const Entity &_entity, return; } - // Subscribe to commands - std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; + // Subscribe to model commands + std::string modelTopic{ + "/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()); + modelTopic = _sdf->Get("topic"); + modelTopic = transport::TopicUtils::AsValidTopic(modelTopic); - ignmsg << "VelocityControl subscribing to twist messages on [" << topic << "]" + this->dataPtr->node.Subscribe( + modelTopic, &VelocityControlPrivate::OnCmdVel, this->dataPtr.get()); + ignmsg << "VelocityControl subscribing to twist messages on [" + << modelTopic << "]" << std::endl; + + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto ptr = const_cast(_sdf.get()); + + if (!ptr->HasElement("link_name")) + return; + + sdf::ElementPtr sdfElem = ptr->GetElement("link_name"); + while (sdfElem) + { + this->dataPtr->linkNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("link_name"); + } + + // Subscribe to link commands + for (const auto &linkName : this->dataPtr->linkNames) + { + std::string linkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/link/" + linkName + "/cmd_vel"}; + linkTopic = transport::TopicUtils::AsValidTopic(linkTopic); + this->dataPtr->node.Subscribe( + linkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); + ignmsg << "VelocityControl subscribing to twist messages on [" + << linkTopic << "]" + << std::endl; + } } ////////////////////////////////////////////////// @@ -116,11 +176,11 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, return; // update angular velocity of model - auto angularVel = + auto modelAngularVel = _ecm.Component( this->dataPtr->model.Entity()); - if (angularVel == nullptr) + if (modelAngularVel == nullptr) { _ecm.CreateComponent( this->dataPtr->model.Entity(), @@ -128,16 +188,16 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - *angularVel = + *modelAngularVel = components::AngularVelocityCmd({this->dataPtr->angularVelocity}); } // update linear velocity of model - auto linearVel = + auto modelLinearVel = _ecm.Component( this->dataPtr->model.Entity()); - if (linearVel == nullptr) + if (modelLinearVel == nullptr) { _ecm.CreateComponent( this->dataPtr->model.Entity(), @@ -145,9 +205,84 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - *linearVel = + *modelLinearVel = components::LinearVelocityCmd({this->dataPtr->linearVelocity}); } + + // If there are links, create link components + // If the link hasn't been identified yet, look for it + auto modelName = this->dataPtr->model.Name(_ecm); + + if (this->dataPtr->linkNames.empty()) + return; + + // find all the link entity ids + if (this->dataPtr->links.size() != this->dataPtr->linkNames.size()) + { + for (const auto &linkName : this->dataPtr->linkNames) + { + if (this->dataPtr->links.find(linkName) == this->dataPtr->links.end()) + { + Entity link = this->dataPtr->model.LinkByName(_ecm, linkName); + if (link != kNullEntity) + this->dataPtr->links.insert({linkName, link}); + else + { + ignwarn << "Failed to find link [" << linkName + << "] for model [" << modelName << "]" << std::endl; + } + } + } + } + + // update link velocities + for (const auto& [linkName, angularVel] : this->dataPtr->angularVelocities) + { + auto it = this->dataPtr->links.find(linkName); + if (it != this->dataPtr->links.end()) + { + auto linkAngularVelComp = + _ecm.Component(it->second); + if (!linkAngularVelComp) + { + _ecm.CreateComponent(it->second, + components::AngularVelocityCmd({angularVel})); + } + else + { + *linkAngularVelComp = components::AngularVelocityCmd(angularVel); + } + } + else + { + ignwarn << "No link found for angular velocity cmd [" + << linkName << "]" << std::endl; + } + } + + for (const auto& [linkName, linearVel] : this->dataPtr->linearVelocities) + { + auto it = this->dataPtr->links.find(linkName); + if (it != this->dataPtr->links.end()) + { + auto linkLinearVelComp = + _ecm.Component(it->second); + if (!linkLinearVelComp) + { + _ecm.CreateComponent(it->second, + components::LinearVelocityCmd({linearVel})); + } + else + { + *linkLinearVelComp = components::LinearVelocityCmd(linearVel); + } + } + else + { + ignwarn << "No link found for linear velocity cmd [" + << linkName << "]" << std::endl; + } + } } ////////////////////////////////////////////////// @@ -159,10 +294,12 @@ void VelocityControl::PostUpdate(const UpdateInfo &_info, if (_info.paused) return; + // update model velocities this->dataPtr->UpdateVelocity(_info, _ecm); + // update link velocities + this->dataPtr->UpdateLinkVelocity(_info, _ecm); } - ////////////////////////////////////////////////// void VelocityControlPrivate::UpdateVelocity( const ignition::gazebo::UpdateInfo &/*_info*/, @@ -170,18 +307,27 @@ void VelocityControlPrivate::UpdateVelocity( { IGN_PROFILE("VeocityControl::UpdateVelocity"); - double linVel; - double angVel; + std::lock_guard lock(this->mutex); + this->linearVelocity = msgs::Convert(this->targetVel.linear()); + this->angularVelocity = msgs::Convert(this->targetVel.angular()); +} + +////////////////////////////////////////////////// +void VelocityControlPrivate::UpdateLinkVelocity( + const ignition::gazebo::UpdateInfo &/*_info*/, + const ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ + IGN_PROFILE("VelocityControl::UpdateLinkVelocity"); + + std::lock_guard lock(this->mutex); + for (const auto& [linkName, msg] : this->linkVels) { - std::lock_guard lock(this->mutex); - linVel = this->targetVel.linear().x(); - angVel = this->targetVel.angular().z(); + auto linearVel = msgs::Convert(msg.linear()); + auto angularVel = msgs::Convert(msg.angular()); + this->linearVelocities[linkName] = linearVel; + this->angularVelocities[linkName] = angularVel; } - - 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); + this->linkVels.clear(); } ////////////////////////////////////////////////// @@ -191,6 +337,21 @@ void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg) this->targetVel = _msg; } +////////////////////////////////////////////////// +void VelocityControlPrivate::OnLinkCmdVel(const msgs::Twist &_msg, + const transport::MessageInfo &_info) +{ + std::lock_guard lock(this->mutex); + for (const auto &linkName : this->linkNames) + { + if (_info.Topic().find("/" + linkName + "/cmd_vel") != std::string::npos) + { + this->linkVels.insert({linkName, _msg}); + break; + } + } +} + IGNITION_ADD_PLUGIN(VelocityControl, ignition::gazebo::System, VelocityControl::ISystemConfigure, diff --git a/test/integration/velocity_control_system.cc b/test/integration/velocity_control_system.cc index df1354b924..5eb7722440 100644 --- a/test/integration/velocity_control_system.cc +++ b/test/integration/velocity_control_system.cc @@ -21,6 +21,7 @@ #include #include +#include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Pose.hh" @@ -126,6 +127,112 @@ class VelocityControlTest : public ::testing::TestWithParam EXPECT_GT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z()) << i; } } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + protected: void TestPublishLinkCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + // currently only tpe supports link velocity cmds + serverConfig.SetPhysicsEngine("ignition-physics-tpe-plugin"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle link poses + test::Relay testSystem; + + std::vector modelPoses; + std::vector linkPoses; + testSystem.OnPostUpdate([&linkPoses, &modelPoses]( + const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto modelId = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle_blue")); + EXPECT_NE(kNullEntity, modelId); + + auto modelPoseComp = _ecm.Component(modelId); + ASSERT_NE(nullptr, modelPoseComp); + + modelPoses.push_back(modelPoseComp->Data()); + + auto linkId = _ecm.EntityByComponents( + components::Link(), + components::Name("caster")); + EXPECT_NE(kNullEntity, linkId); + + auto linkPoseComp = _ecm.Component(linkId); + ASSERT_NE(nullptr, linkPoseComp); + + linkPoses.push_back(linkPoseComp->Data()); + + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, modelPoses.size()); + + for (const auto &pose : modelPoses) + { + EXPECT_EQ(modelPoses[0], pose); + } + for (const auto &pose : linkPoses) + { + EXPECT_EQ(linkPoses[0], pose); + } + + // Publish command and check that link moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + + msgs::Twist msg; + + const double desiredLinVel = 10.5; + const double desiredAngVel = 0.2; + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, desiredAngVel)); + pub.Publish(msg); + + // Give some time for message to be received + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, modelPoses.size()); + ASSERT_EQ(4000u, linkPoses.size()); + + // verify that the model is stationary + for (unsigned int i = 1001; i < modelPoses.size(); ++i) + { + EXPECT_EQ(modelPoses[0], modelPoses[i]); + } + + // verify that the link is moving in +x and rotating about its origin + for (unsigned int i = 1001; i < linkPoses.size(); ++i) + { + EXPECT_GT(linkPoses[i].Pos().X(), linkPoses[i-1].Pos().X()) << i; + EXPECT_NEAR(linkPoses[i].Pos().Y(), linkPoses[i-1].Pos().Y(), 1e-5); + EXPECT_NEAR(linkPoses[i].Pos().Z(), linkPoses[i-1].Pos().Z(), 1e-5); + EXPECT_NEAR(linkPoses[i].Rot().Euler().X(), + linkPoses[i-1].Rot().Euler().X(), 1e-5) << i; + EXPECT_NEAR(linkPoses[i].Rot().Euler().Y(), + linkPoses[i-1].Rot().Euler().Y(), 1e-5) << i; + EXPECT_GT(linkPoses[i].Rot().Euler().Z(), + linkPoses[i-1].Rot().Euler().Z()) << i; + } + } }; ///////////////////////////////////////////////// @@ -136,6 +243,14 @@ TEST_P(VelocityControlTest, PublishCmd) "/model/vehicle_blue/cmd_vel"); } +///////////////////////////////////////////////// +TEST_P(VelocityControlTest, PublishLinkCmd) +{ + TestPublishLinkCmd( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/velocity_control.sdf", + "/model/vehicle_blue/link/caster/cmd_vel"); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, VelocityControlTest, ::testing::Range(1, 2)); diff --git a/test/worlds/velocity_control.sdf b/test/worlds/velocity_control.sdf index 2e104e5f08..bae5e14df4 100644 --- a/test/worlds/velocity_control.sdf +++ b/test/worlds/velocity_control.sdf @@ -310,6 +310,7 @@ + caster From 50317022e935217e482c11b4d06773213ae3fed9 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 23 Apr 2021 17:23:11 -0700 Subject: [PATCH 4/6] Don't store duplicate ComponentTypeId in ECM (#751) Signed-off-by: Louise Poubel --- src/EntityComponentManager.cc | 52 +++++++++++++++--------------- src/EntityComponentManager_TEST.cc | 5 +++ 2 files changed, 31 insertions(+), 26 deletions(-) diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index b7aa6e0a6a..8562cfc072 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -87,7 +87,7 @@ class ignition::gazebo::EntityComponentManagerPrivate /// NOTE: Any modification of this data structure must be followed /// by setting `entityComponentsDirty` to true. public: std::unordered_map> entityComponents; + std::unordered_map> entityComponents; /// \brief A vector of iterators to evenly distributed spots in the /// `entityComponents` map. Threads in the `State` function use this @@ -95,7 +95,7 @@ class ignition::gazebo::EntityComponentManagerPrivate /// is recalculated if `entityComponents` is changed (when /// `entityComponentsDirty` == true). public: std::vector>::iterator> + std::unordered_map>::iterator> entityComponentIterators; /// \brief A mutex to protect newly created entityes. @@ -272,8 +272,7 @@ void EntityComponentManager::ProcessRemoveEntityRequests() { for (const auto &key : entityIter->second) { - this->dataPtr->components.at(key.second.first)->Remove( - key.second.second); + this->dataPtr->components.at(key.first)->Remove(key.second); } // Remove the entry in the entityComponent map @@ -384,12 +383,14 @@ ComponentState EntityComponentManager::ComponentState(const Entity _entity, if (typeKey == ecIter->second.end()) return result; - if (this->dataPtr->oneTimeChangedComponents.find(typeKey->second) != + ComponentKey key{_typeId, typeKey->second}; + + if (this->dataPtr->oneTimeChangedComponents.find(key) != this->dataPtr->oneTimeChangedComponents.end()) { result = ComponentState::OneTimeChange; } - else if (this->dataPtr->periodicChangedComponents.find(typeKey->second) != + else if (this->dataPtr->periodicChangedComponents.find(key) != this->dataPtr->periodicChangedComponents.end()) { result = ComponentState::PeriodicChange; @@ -496,7 +497,7 @@ ComponentKey EntityComponentManager::CreateComponentImplementation( ComponentKey componentKey{_componentTypeId, componentIdPair.first}; this->dataPtr->entityComponents[_entity].insert( - {_componentTypeId, componentKey}); + {_componentTypeId, componentIdPair.first}); this->dataPtr->oneTimeChangedComponents.insert(componentKey); this->dataPtr->entityComponentsDirty = true; @@ -541,7 +542,7 @@ ComponentId EntityComponentManager::EntityComponentIdFromType( auto typeIter = ecIter->second.find(_type); if (typeIter != ecIter->second.end()) - return typeIter->second.second; + return typeIter->second; return -1; } @@ -559,8 +560,8 @@ const components::BaseComponent auto typeIter = ecIter->second.find(_type); if (typeIter != ecIter->second.end()) - return this->dataPtr->components.at(typeIter->second.first)->Component( - typeIter->second.second); + return this->dataPtr->components.at(_type)->Component( + typeIter->second); return nullptr; } @@ -576,8 +577,7 @@ components::BaseComponent *EntityComponentManager::ComponentImplementation( auto typeIter = ecIter->second.find(_type); if (typeIter != ecIter->second.end()) - return this->dataPtr->components.at(typeIter->second.first)->Component( - typeIter->second.second); + return this->dataPtr->components.at(_type)->Component(typeIter->second); return nullptr; } @@ -768,16 +768,14 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, for (const ComponentTypeId type : types) { // If the entity does not have the component, continue - std::unordered_map::iterator typeIter = - iter->second.find(type); + auto typeIter = iter->second.find(type); if (typeIter == iter->second.end()) { continue; } - ComponentKey comp = (typeIter->second); auto compMsg = entityMsg->add_components(); - auto compBase = this->ComponentImplementation(_entity, comp.first); + auto compBase = this->ComponentImplementation(_entity, type); compMsg->set_type(compBase->TypeId()); std::ostringstream ostr; @@ -833,16 +831,16 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, // Empty means all types for (const ComponentTypeId type : types) { - std::unordered_map::iterator typeIter = - iter->second.find(type); + auto typeIter = iter->second.find(type); if (typeIter == iter->second.end()) { continue; } - ComponentKey comp = typeIter->second; const components::BaseComponent *compBase = - this->ComponentImplementation(_entity, comp.first); + this->ComponentImplementation(_entity, type); + + ComponentKey comp = {type, typeIter->second}; // If not sending full state, skip unchanged components if (!_full && @@ -1303,20 +1301,22 @@ void EntityComponentManager::SetChanged( if (typeIter == ecIter->second.end()) return; + ComponentKey key{_type, typeIter->second}; + if (_c == ComponentState::PeriodicChange) { - this->dataPtr->periodicChangedComponents.insert(typeIter->second); - this->dataPtr->oneTimeChangedComponents.erase(typeIter->second); + this->dataPtr->periodicChangedComponents.insert(key); + this->dataPtr->oneTimeChangedComponents.erase(key); } else if (_c == ComponentState::OneTimeChange) { - this->dataPtr->periodicChangedComponents.erase(typeIter->second); - this->dataPtr->oneTimeChangedComponents.insert(typeIter->second); + this->dataPtr->periodicChangedComponents.erase(key); + this->dataPtr->oneTimeChangedComponents.insert(key); } else { - this->dataPtr->periodicChangedComponents.erase(typeIter->second); - this->dataPtr->oneTimeChangedComponents.erase(typeIter->second); + this->dataPtr->periodicChangedComponents.erase(key); + this->dataPtr->oneTimeChangedComponents.erase(key); } } diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 0ef3587247..75228f3803 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2158,6 +2158,11 @@ TEST_P(EntityComponentManagerFixture, SetEntityCreateOffset) manager.SetEntityCreateOffset(1000); Entity entity2 = manager.CreateEntity(); EXPECT_EQ(1001u, entity2); + + // Apply a lower offset, prints warning but goes through. + manager.SetEntityCreateOffset(500); + Entity entity3 = manager.CreateEntity(); + EXPECT_EQ(501u, entity3); } // Run multiple times. We want to make sure that static globals don't cause From 361982a8a49c3960b179d91de0cfd9220432fe18 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 26 Apr 2021 14:54:29 -0700 Subject: [PATCH 5/6] Fix macOS build: components::Name in benchmark (#784) Signed-off-by: Louise Poubel Co-authored-by: Steve Peters --- test/benchmark/each.cc | 38 +++++++++++++++++++------------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/test/benchmark/each.cc b/test/benchmark/each.cc index e5f5f5336a..a386633c4c 100644 --- a/test/benchmark/each.cc +++ b/test/benchmark/each.cc @@ -54,13 +54,13 @@ class EntityComponentManagerFixture: public benchmark::Fixture { Entity worldEntity = mgr->CreateEntity(); mgr->CreateComponent(worldEntity, World()); - mgr->CreateComponent(worldEntity, Name("world_name")); + mgr->CreateComponent(worldEntity, components::Name("world_name")); } for (int i = 0; i < _nonmatchingEntityCount; ++i) { Entity worldEntity = mgr->CreateEntity(); - mgr->CreateComponent(worldEntity, Name("world_name")); + mgr->CreateComponent(worldEntity, components::Name("world_name")); } } @@ -77,8 +77,8 @@ BENCHMARK_DEFINE_F(EntityComponentManagerFixture, EachNoCache) { int entitiesMatched = 0; - mgr->EachNoCache( - [&](const Entity &, const World *, const Name *)->bool + mgr->EachNoCache( + [&](const Entity &, const World *, const components::Name *)->bool { entitiesMatched++; return true; @@ -102,8 +102,8 @@ BENCHMARK_DEFINE_F(EntityComponentManagerFixture, EachCache) { int entitiesMatched = 0; - mgr->Each( - [&](const Entity &, const World *, const Name *)->bool + mgr->Each( + [&](const Entity &, const World *, const components::Name *)->bool { entitiesMatched++; return true; @@ -131,7 +131,7 @@ class ManyComponentFixture: public benchmark::Fixture for (int i = 0; i < _entityCount; ++i) { Entity entity = mgr->CreateEntity(); - mgr->CreateComponent(entity, Name("world_name")); + mgr->CreateComponent(entity, components::Name("world_name")); mgr->CreateComponent(entity, AngularVelocity()); mgr->CreateComponent(entity, WorldAngularVelocity()); mgr->CreateComponent(entity, Inertial()); @@ -158,8 +158,8 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each1ComponentCache) { int entitiesMatched = 0; - mgr->Each( - [&](const Entity &, const Name *)->bool + mgr->Each( + [&](const Entity &, const components::Name *)->bool { entitiesMatched++; return true; @@ -184,13 +184,13 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each5ComponentCache) { int entitiesMatched = 0; - mgr->EachEach( [&](const Entity &, - const Name *, + const components::Name *, const AngularVelocity *, const Inertial *, const LinearAcceleration *, @@ -219,7 +219,7 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each10ComponentCache) { int entitiesMatched = 0; - mgr->EachEach( [&](const Entity &, - const Name *, + const components::Name *, const AngularVelocity *, const WorldAngularVelocity *, const Inertial *, @@ -264,8 +264,8 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each1ComponentNoCache) { int entitiesMatched = 0; - mgr->EachNoCache( - [&](const Entity &, const Name *)->bool + mgr->EachNoCache( + [&](const Entity &, const components::Name *)->bool { entitiesMatched++; return true; @@ -290,13 +290,13 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each5ComponentNoCache) { int entitiesMatched = 0; - mgr->EachNoCacheEachNoCache( [&](const Entity &, - const Name *, + const components::Name *, const AngularVelocity *, const Inertial *, const LinearAcceleration *, @@ -325,7 +325,7 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each10ComponentNoCache) { int entitiesMatched = 0; - mgr->EachNoCacheEachNoCache( [&](const Entity &, - const Name *, + const components::Name *, const AngularVelocity *, const WorldAngularVelocity *, const Inertial *, From 803130ea8174a9503352e52d742d50f2f11b4786 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 28 Apr 2021 17:57:25 -0700 Subject: [PATCH 6/6] Bump ign-physics version to 3.2 (#792) Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4fc229248f..d199fe7b17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,7 +87,7 @@ ign_find_package (Qt5 #-------------------------------------- # Find ignition-physics -ign_find_package(ignition-physics3 VERSION 3.1 +ign_find_package(ignition-physics3 VERSION 3.2 COMPONENTS mesh sdf