Merge branch 'fix_fullscreen' of github.com:mohamedsayed18/ign-gazebo…

… into fix_fullscreen

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 <ignition/math/Vector3.hh>
+
+#include <ignition/gazebo/config.hh>
+
+#include <ignition/gazebo/components/Factory.hh>
+#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<math::Vector3d, class AngularVelocityCmdTag>;
+  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<math::Vector3d, class WorldAngularVelocityCmdTag>;
+  IGN_GAZEBO_REGISTER_COMPONENT(
+    "ign_gazebo_components.WorldAngularVelocityCmd", WorldAngularVelocityCmd)
+}
+}
+}
+}
+
+#endif

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 <ignition/math/Vector3.hh>
+
+#include <ignition/gazebo/config.hh>
+
+#include <ignition/gazebo/components/Factory.hh>
+#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 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<math::Vector3d, class WorldLinearVelocityCmdTag>;
+  IGN_GAZEBO_REGISTER_COMPONENT(
+      "ign_gazebo_components.WorldLinearVelocityCmd", WorldLinearVelocityCmd)
+}
+}
+}
+}
+
+#endif

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)

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)

src/systems/physics/Physics.cc

@@ -29,6 +29,7 @@
 #include <ignition/common/SystemPaths.hh>
 #include <ignition/math/AxisAlignedBox.hh>
 #include <ignition/math/eigen3/Conversions.hh>
+#include <ignition/math/Vector3.hh>
 #include <ignition/physics/config.hh>
 #include <ignition/physics/FeatureList.hh>
 #include <ignition/physics/FeaturePolicy.hh>
@@ -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<Entity, JointVelocityCommandPtrType>
       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<Entity, FreeGroupVelocityCmdPtrType>
+      entityWorldVelocityCommandMap;
+
   //////////////////////////////////////////////////
   // Meshes
 
@@ -1316,6 +1338,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
         return true;
       });
 
+  // Update model pose
   _ecm.Each<components::Model, components::WorldPoseCmd>(
       [&](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<components::Model, components::AngularVelocityCmd>(
+      [&](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<components::Pose>(_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<components::Model, components::LinearVelocityCmd>(
+      [&](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<components::Pose>(_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

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}
+)