Allows to externally disable vehicle via VehicleInputControlBus. (#301)

Signed-off-by: Michał Pełka <[email protected]>

Gems/ROS2/Code/Source/VehicleDynamics/VehicleInputControlBus.h → Gems/ROS2/Code/Include/ROS2/VehicleDynamics/VehicleInputControlBus.h

@@ -46,6 +46,10 @@ namespace VehicleDynamics
         //! Speed input version which is relative to limits.
         //! @param speedMps is -1 to 1, which applies as a fraction of vehicle model speed limits.
         virtual void SetTargetLinearSpeedFraction(float speedFraction) = 0;
+
+        //! Disable vehicle dynamics
+        //! @param is_disable if set true no torque will be applied
+        virtual void SetDisableVehicleDynamics(bool is_disable) = 0;
     };
 
     using VehicleInputControlRequestBus = AZ::EBus<VehicleInputControlRequests>;

Gems/ROS2/Code/Source/RobotControl/Controllers/AckermannController/AckermannControlComponent.cpp

@@ -7,7 +7,7 @@
  */
 
 #include "AckermannControlComponent.h"
-#include "VehicleDynamics/VehicleInputControlBus.h"
+#include "ROS2/VehicleDynamics/VehicleInputControlBus.h"
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
 #include <AzFramework/Physics/RigidBodyBus.h>

Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/AckermannDriveModel.cpp

@@ -93,6 +93,10 @@ namespace VehicleDynamics
     // TODO - speed and steering handling is quite similar, possible to refactor?
     void AckermannDriveModel::ApplySteering(float steering, uint64_t deltaTimeNs)
     {
+        if (m_disabled)
+        {
+            return;
+        }
         if (m_steeringData.empty())
         {
             AZ_Warning("ApplySteering", false, "Cannot apply steering since no steering elements are defined in the model");
@@ -112,6 +116,10 @@ namespace VehicleDynamics
 
     void AckermannDriveModel::ApplySpeed(float speed, uint64_t deltaTimeNs)
     {
+        if (m_disabled)
+        {
+            return;
+        }
         if (m_driveWheelsData.empty())
         {
             AZ_Warning("ApplySpeed", false, "Cannot apply speed since no diving wheels are defined in the model");
@@ -151,4 +159,10 @@ namespace VehicleDynamics
             Physics::RigidBodyRequestBus::Event(wheelEntity, &Physics::RigidBodyRequests::ApplyAngularImpulse, transformedTorqueVector);
         }
     }
+
+    void AckermannDriveModel::SetDisabled(bool is_disabled)
+    {
+        m_disabled = is_disabled;
+    }
+
 } // namespace VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/DriveModels/AckermannDriveModel.h

@@ -30,6 +30,8 @@ namespace VehicleDynamics
 
         static void Reflect(AZ::ReflectContext* context);
 
+        void SetDisabled(bool is_disabled);
+
     private:
         void ApplySteering(float steering, uint64_t deltaTimeNs);
         void ApplySpeed(float speed, uint64_t deltaTimeNs);
@@ -40,6 +42,7 @@ namespace VehicleDynamics
         AZStd::vector<SteeringDynamicsData> m_steeringData;
         PidConfiguration m_steeringPid;
         PidConfiguration m_speedPid;
+        bool m_disabled{ false };
         float m_steeringDeadZone = 0.01;
     };
 } // namespace VehicleDynamics

Gems/ROS2/Code/Source/VehicleDynamics/ManualControlEventHandler.h

@@ -7,7 +7,7 @@
  */
 #pragma once
 
-#include "VehicleDynamics/VehicleInputControlBus.h"
+#include "ROS2/VehicleDynamics/VehicleInputControlBus.h"
 #include <StartingPointInput/InputEventNotificationBus.h>
 
 // TODO - plenty of boilerplate code, seems somewhat redundant since it would be better to be able to map inputs directly

Gems/ROS2/Code/Source/VehicleDynamics/VehicleModelComponent.cpp

@@ -98,6 +98,11 @@ namespace VehicleDynamics
         AZ_Error("SetTargetAccelerationFraction", false, "Not implemented");
     }
 
+    void VehicleModelComponent::SetDisableVehicleDynamics(bool is_disable)
+    {
+        m_driveModel.SetDisabled(is_disable);
+    }
+
     void VehicleModelComponent::SetTargetSteering(float steering)
     {
         auto limitedSteering = VehicleModelLimits::LimitValue(steering, m_vehicleLimits.m_steeringLimit);

Gems/ROS2/Code/Source/VehicleDynamics/VehicleModelComponent.h

@@ -8,9 +8,9 @@
 #pragma once
 
 #include "ManualControlEventHandler.h"
+#include "ROS2/VehicleDynamics/VehicleInputControlBus.h"
 #include "VehicleConfiguration.h"
 #include "VehicleDynamics/DriveModels/AckermannDriveModel.h"
-#include "VehicleInputControlBus.h"
 #include "VehicleInputsState.h"
 #include "VehicleModelLimits.h"
 #include <AzCore/Component/Component.h>
@@ -44,6 +44,7 @@ namespace VehicleDynamics
         void SetTargetAccelerationFraction(float accelerationFraction) override;
         void SetTargetSteeringFraction(float steeringFraction) override;
         void SetTargetLinearSpeedFraction(float speedFraction) override;
+        void SetDisableVehicleDynamics(bool is_disable) override;
 
         ManualControlEventHandler m_manualControlEventHandler;
         VehicleConfiguration m_vehicleConfiguration;

Gems/ROS2/Code/ros2_files.cmake

@@ -83,7 +83,6 @@ set(FILES
         Source/VehicleDynamics/DriveModel.cpp
         Source/VehicleDynamics/DriveModel.h
         Source/VehicleDynamics/ManualControlEventHandler.h
-        Source/VehicleDynamics/VehicleInputControlBus.h
         Source/VehicleDynamics/VehicleInputsState.h
         Source/VehicleDynamics/Utilities.cpp
         Source/VehicleDynamics/Utilities.h

Gems/ROS2/Code/ros2_header_files.cmake

@@ -13,4 +13,5 @@ set(FILES
         Include/ROS2/Utilities/ROS2Conversions.h
         Include/ROS2/Utilities/ROS2Names.h
         Include/ROS2/VehicleDynamics/DriveModels/PidConfiguration.h
+        Include/ROS2/VehicleDynamics/VehicleInputControlBus.h
         )