formatting

Signed-off-by: Adam Dabrowski <[email protected]>

Project/Gem/Include/ROSConDemo/ROSConDemoBus.h

@@ -14,8 +14,7 @@ namespace ROSConDemo
         // Put your public methods here
     };
 
-    class ROSConDemoBusTraits
-        : public AZ::EBusTraits
+    class ROSConDemoBusTraits : public AZ::EBusTraits
     {
     public:
         //////////////////////////////////////////////////////////////////////////

Project/Gem/Source/ApplePicker/AppleDetectionGroundTruth.cpp

@@ -7,9 +7,9 @@
  */
 
 #include "AppleDetectionGroundTruth.h"
+#include "ROS2/Utilities/ROS2Conversions.h"
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/ROS2Bus.h>
-#include "ROS2/Utilities/ROS2Conversions.h"
 #include <ROS2/Utilities/ROS2Names.h>
 #include <rclcpp/qos.hpp>
 

Project/Gem/Source/ApplePicker/AppleDetectionGroundTruth.h

@@ -8,10 +8,10 @@
 #pragma once
 
 #include "PickingStructs.h"
+#include <AzCore/std/containers/vector.h>
 #include <rclcpp/rclcpp.hpp>
-#include <vision_msgs/msg/detection3_d_array.hpp>
 #include <vision_msgs/msg/detection2_d_array.hpp>
-#include <AzCore/std/containers/vector.h>
+#include <vision_msgs/msg/detection3_d_array.hpp>
 
 namespace AppleKraken
 {

Project/Gem/Source/ApplePicker/ApplePickerComponent.cpp

@@ -354,7 +354,7 @@ namespace AppleKraken
             appleTasks.begin(),
             appleTasks.end(),
             [globalBox](const PickAppleTask& a, const PickAppleTask& b) -> bool
-            {   // a is further than b from the globalBox
+            { // a is further than b from the globalBox
                 return globalBox.GetDistance(a.m_middle) > globalBox.GetDistance(b.m_middle);
             });
 
@@ -368,4 +368,3 @@ namespace AppleKraken
         AZ_Printf("ApplePickerComponent", "There are %d apples in reach box \n", m_currentAppleTasks.size());
     }
 } // namespace AppleKraken
-

Project/Gem/Source/ApplePicker/ApplePickerComponent.h

@@ -7,8 +7,8 @@
  */
 #pragma once
 
-#include "ApplePickingNotifications.h"
 #include "AppleDetectionGroundTruth.h"
+#include "ApplePickingNotifications.h"
 #include "ApplePickingRequests.h"
 #include <AzCore/Component/Component.h>
 #include <AzCore/Component/TickBus.h>

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.cpp

@@ -8,15 +8,15 @@
 
 #include "KrakenEffectorComponent.h"
 #include "ApplePickingNotifications.h"
-#include "PickingStructs.h"
 #include "ManipulatorRequestBus.h"
+#include "PickingStructs.h"
 #include <AzCore/Component/TransformBus.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
 #include <AzCore/Serialization/SerializeContext.h>
-#include <LmbrCentral/Shape/BoxShapeComponentBus.h>
-#include <AzFramework/Physics/RigidBodyBus.h>
 #include <AzFramework/Physics/Common/PhysicsSimulatedBody.h>
+#include <AzFramework/Physics/RigidBodyBus.h>
+#include <LmbrCentral/Shape/BoxShapeComponentBus.h>
 namespace AppleKraken
 {
     namespace DebugStateTransit
@@ -72,23 +72,28 @@ namespace AppleKraken
         ApplePickingRequestBus::Handler::BusConnect(GetEntityId());
         AZ::TickBus::Handler::BusConnect();
 
-        m_onTriggerHandleBeginHandler = AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler([&]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle,[[maybe_unused]] const AzPhysics::TriggerEvent& event){
-
-            const AZ::EntityId& e1 = event.m_otherBody->GetEntityId();
-            const AZ::EntityId& e2 = event.m_triggerBody->GetEntityId();
-            [[maybe_unused]]const AZ::EntityId& collideToEntityId =  m_appleProbe == e1 ? e2 : e1;
-            //AzPhysics::SimulatedBody* collideToEntityId = this->GetEntityId() == e1 ?  event.m_triggerBody : event.m_otherBody;}
-            if (m_currentTask.m_appleEntityId == collideToEntityId){
-                AZ_Printf("m_onTriggerHandleBeginHandler", "=================m_onTriggerHandle to Apple!====================");
-                if ( m_effectorState == EffectorState::PICKING)
+        m_onTriggerHandleBeginHandler = AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler(
+            [&]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, [[maybe_unused]] const AzPhysics::TriggerEvent& event)
+            {
+                const AZ::EntityId& e1 = event.m_otherBody->GetEntityId();
+                const AZ::EntityId& e2 = event.m_triggerBody->GetEntityId();
+                [[maybe_unused]] const AZ::EntityId& collideToEntityId = m_appleProbe == e1 ? e2 : e1;
+                // AzPhysics::SimulatedBody* collideToEntityId = this->GetEntityId() == e1 ?  event.m_triggerBody : event.m_otherBody;}
+                if (m_currentTask.m_appleEntityId == collideToEntityId)
                 {
-                    // start picking the apple
-                    BeginTransitionIfAcceptable(EffectorState::RETRIEVING);
+                    AZ_Printf("m_onTriggerHandleBeginHandler", "=================m_onTriggerHandle to Apple!====================");
+                    if (m_effectorState == EffectorState::PICKING)
+                    {
+                        // start picking the apple
+                        BeginTransitionIfAcceptable(EffectorState::RETRIEVING);
+                    }
                 }
-            }
-            AZ_Printf("m_onTriggerHandleBeginHandler", "m_onTriggerHandleBeginHandler %s : %s \n", m_currentTask.m_appleEntityId.ToString().c_str(), collideToEntityId.ToString().c_str());
-        });
-
+                AZ_Printf(
+                    "m_onTriggerHandleBeginHandler",
+                    "m_onTriggerHandleBeginHandler %s : %s \n",
+                    m_currentTask.m_appleEntityId.ToString().c_str(),
+                    collideToEntityId.ToString().c_str());
+            });
     }
 
     void KrakenEffectorComponent::Deactivate()
@@ -101,12 +106,13 @@ namespace AppleKraken
     {
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
         {
-            serialize->Class<KrakenEffectorComponent, AZ::Component>()->Version(3)->Field(
-                "ReachEntity", &KrakenEffectorComponent::m_reachEntity)
+            serialize->Class<KrakenEffectorComponent, AZ::Component>()
+                ->Version(3)
+                ->Field("ReachEntity", &KrakenEffectorComponent::m_reachEntity)
                 ->Field("ManipulatorEntity", &KrakenEffectorComponent::m_manipulatorEntity)
                 ->Field("AppleProbe", &KrakenEffectorComponent::m_appleProbe)
-                ->Field("RootManipulatorFreeze",&KrakenEffectorComponent::m_rootEntityToFreeze)
-                ->Field("BaseLinkToKinematic",&KrakenEffectorComponent::m_baseLinkToKinematic);
+                ->Field("RootManipulatorFreeze", &KrakenEffectorComponent::m_rootEntityToFreeze)
+                ->Field("BaseLinkToKinematic", &KrakenEffectorComponent::m_baseLinkToKinematic);
 
             if (AZ::EditContext* ec = serialize->GetEditContext())
             {
@@ -125,20 +131,17 @@ namespace AppleKraken
                         "Entity with manipulator",
                         "The entity that has a component handling events from ManipulatorRequestBus")
                     ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &KrakenEffectorComponent::m_appleProbe,
-                        "Entity to probe apples",
-                        "Sucking collider")
+                        AZ::Edit::UIHandlers::Default, &KrakenEffectorComponent::m_appleProbe, "Entity to probe apples", "Sucking collider")
                     ->DataElement(
                         AZ::Edit::UIHandlers::Default,
                         &KrakenEffectorComponent::m_rootEntityToFreeze,
                         "RootManipulatorFreeze",
                         "RootManipulatorFreeze to freeze during robot movement")
                     ->DataElement(
-                            AZ::Edit::UIHandlers::Default,
-                            &KrakenEffectorComponent::m_baseLinkToKinematic,
-                            "BaseLinkToKinematic",
-                            "BaseLinkToKinematic during manipulator movement")
+                        AZ::Edit::UIHandlers::Default,
+                        &KrakenEffectorComponent::m_baseLinkToKinematic,
+                        "BaseLinkToKinematic",
+                        "BaseLinkToKinematic during manipulator movement")
                     ->Attribute(AZ::Edit::Attributes::AutoExpand, true);
             }
         }
@@ -208,7 +211,6 @@ namespace AppleKraken
     {
         AZ_TracePrintf("KrakenEffectorComponent", "FinishPicking\n");
         BeginTransitionIfAcceptable(EffectorState::IDLE);
-
     }
 
     PickingState KrakenEffectorComponent::GetEffectorState()
@@ -286,7 +288,6 @@ namespace AppleKraken
         ManipulatorRequestBus::Event(m_manipulatorEntity, &ManipulatorRequest::Retrieve);
         // TODO - also handle picking failed
         ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::ApplePicked);
-
     }
 
     void KrakenEffectorComponent::OnAppleRetrieved()
@@ -294,79 +295,91 @@ namespace AppleKraken
         ApplePickingNotificationBus::Broadcast(&ApplePickingNotifications::AppleRetrieved);
     }
 
-    void KrakenEffectorComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) {
+    void KrakenEffectorComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    {
         m_currentStateTransitionTime += deltaTime;
-        if (m_effectorState == EffectorState::PICKING && m_currentStateTransitionTime > 5) {
+        if (m_effectorState == EffectorState::PICKING && m_currentStateTransitionTime > 5)
+        {
             AZ_Printf("m_onTriggerHandleBeginHandler", "---------------Failed to retrieve apple--------------------\n");
             BeginTransitionIfAcceptable(EffectorState::RETRIEVING);
         }
 
-        if (m_effectorState == EffectorState::RETRIEVING) {
+        if (m_effectorState == EffectorState::RETRIEVING)
+        {
             int status = -1;
             ManipulatorRequestBus::EventResult(status, m_manipulatorEntity, &ManipulatorRequest::GetStatus);
-            if (status == 10) {
+            if (status == 10)
+            {
                 BeginTransitionIfAcceptable(EffectorState::PREPARED);
             }
-
         }
-        if (m_effectorState == EffectorState::IDLE) {
+        if (m_effectorState == EffectorState::IDLE)
+        {
             LockManipulator(true);
         }
 
-        if (m_effectorState == m_effectorTargetState) { // //TODO - nothing to do in stub version
+        if (m_effectorState == m_effectorTargetState)
+        { // //TODO - nothing to do in stub version
             return;
         }
 
         // State transition
         AZ_TracePrintf(
-                "KrakenEffectorComponent", "%s",
-                DebugStateTransit::StateTransitionString(m_effectorState, m_effectorTargetState).c_str());
+            "KrakenEffectorComponent", "%s", DebugStateTransit::StateTransitionString(m_effectorState, m_effectorTargetState).c_str());
         m_currentStateTransitionTime = 0.0f;
 
         // Update state
         auto previousState = m_effectorState;
         m_effectorState = m_effectorTargetState;
 
-        if (previousState == EffectorState::IDLE && m_effectorState == EffectorState::PREPARED) {
+        if (previousState == EffectorState::IDLE && m_effectorState == EffectorState::PREPARED)
+        {
             OnEffectorReadyForPicking();
         }
 
-        if (previousState == EffectorState::PICKING && m_effectorState == EffectorState::RETRIEVING) {
+        if (previousState == EffectorState::PICKING && m_effectorState == EffectorState::RETRIEVING)
+        {
             OnApplePicked();
         }
 
-        if (previousState == EffectorState::RETRIEVING && m_effectorState == EffectorState::PREPARED) {
+        if (previousState == EffectorState::RETRIEVING && m_effectorState == EffectorState::PREPARED)
+        {
             OnAppleRetrieved();
         }
-        if (!m_registeredCallback) {
-            auto *physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
-            auto *sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        if (!m_registeredCallback)
+        {
+            auto* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
+            auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
             auto [physicScene, physicBody] = physicsSystem->FindAttachedBodyHandleFromEntityId(m_appleProbe);
-            if (physicBody != AzPhysics::InvalidSimulatedBodyHandle && physicScene != AzPhysics::InvalidSceneHandle) {
-                AzPhysics::SimulatedBody *simulated_body = sceneInterface->GetSimulatedBodyFromHandle(physicScene,
-                                                                                                      physicBody);
+            if (physicBody != AzPhysics::InvalidSimulatedBodyHandle && physicScene != AzPhysics::InvalidSceneHandle)
+            {
+                AzPhysics::SimulatedBody* simulated_body = sceneInterface->GetSimulatedBodyFromHandle(physicScene, physicBody);
                 simulated_body->RegisterOnTriggerEnterHandler(m_onTriggerHandleBeginHandler);
                 m_registeredCallback = true;
             }
-
         }
     }
-    void KrakenEffectorComponent::LockManipulator(bool locked) {
+    void KrakenEffectorComponent::LockManipulator(bool locked)
+    {
         AZStd::vector<AZ::EntityId> descendants;
         AZ::TransformBus::EventResult(descendants, m_rootEntityToFreeze, &AZ::TransformBus::Events::GetAllDescendants);
         descendants.push_back(m_rootEntityToFreeze);
-        if (is_manipulator_locked != locked){
-            for (auto &descadant: descendants) {
-                if (locked) {
+        if (is_manipulator_locked != locked)
+        {
+            for (auto& descadant : descendants)
+            {
+                if (locked)
+                {
                     // Lock manipulator, make base_link not kinematic anymore
                     AZ_Printf("KrakenEffectorComponent", "Locking : %s\n", descadant.ToString().c_str());
                     Physics::RigidBodyRequestBus::Event(descadant, &Physics::RigidBodyRequestBus::Events::DisablePhysics);
-                    Physics::RigidBodyRequestBus::Event(m_manipulatorEntity, &Physics::RigidBodyRequestBus::Events::SetKinematic,false);
-
-                } else {
+                    Physics::RigidBodyRequestBus::Event(m_manipulatorEntity, &Physics::RigidBodyRequestBus::Events::SetKinematic, false);
+                }
+                else
+                {
                     // loose manipulator, make base_link kinematic
                     Physics::RigidBodyRequestBus::Event(descadant, &Physics::RigidBodyRequestBus::Events::EnablePhysics);
-                    Physics::RigidBodyRequestBus::Event(m_manipulatorEntity, &Physics::RigidBodyRequestBus::Events::SetKinematic,true);
+                    Physics::RigidBodyRequestBus::Event(m_manipulatorEntity, &Physics::RigidBodyRequestBus::Events::SetKinematic, true);
                 }
             }
             is_manipulator_locked = locked;

Project/Gem/Source/ApplePicker/KrakenEffectorComponent.h

@@ -13,13 +13,13 @@
 #include <AzCore/Component/TickBus.h>
 #include <AzCore/EBus/EBus.h>
 
-#include <AzFramework/Physics/PhysicsSystem.h>
+#include <AzFramework/Physics/Common/PhysicsSceneQueries.h>
+#include <AzFramework/Physics/Common/PhysicsSimulatedBody.h>
+#include <AzFramework/Physics/Configuration/RigidBodyConfiguration.h>
 #include <AzFramework/Physics/PhysicsScene.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
 #include <AzFramework/Physics/Shape.h>
 #include <AzFramework/Physics/SystemBus.h>
-#include <AzFramework/Physics/Common/PhysicsSceneQueries.h>
-#include <AzFramework/Physics/Configuration/RigidBodyConfiguration.h>
-#include <AzFramework/Physics/Common/PhysicsSimulatedBody.h>
 namespace AppleKraken
 {
     //! Component for apple picking effector (manipulator)
@@ -65,8 +65,8 @@ namespace AppleKraken
         AZ::EntityId m_appleProbe;
         AZ::EntityId m_baseLinkToKinematic;
 
-        bool m_registeredCallback{false};
-        bool is_manipulator_locked = {false};
+        bool m_registeredCallback{ false };
+        bool is_manipulator_locked = { false };
         AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler m_onTriggerHandleBeginHandler;
     };
 } // namespace AppleKraken

Project/Gem/Source/ApplePicker/ManipulatorRequestBus.cpp

@@ -10,28 +10,30 @@
 
 namespace AppleKraken
 {
-    void ManipulatorRequestHandler::PickApple(const AZ::Vector3 position){
+    void ManipulatorRequestHandler::PickApple(const AZ::Vector3 position)
+    {
         Call(FN_PickApple, position);
     }
 
-    AZ::Vector3 ManipulatorRequestHandler::GetPosition(){
+    AZ::Vector3 ManipulatorRequestHandler::GetPosition()
+    {
         AZ::Vector3 p;
         CallResult(p, FN_GetPosition);
         return p;
     }
 
-
-    void ManipulatorRequestHandler::Retrieve(){
+    void ManipulatorRequestHandler::Retrieve()
+    {
         Call(FN_Retrieve);
     }
 
-    int ManipulatorRequestHandler::GetStatus(){
+    int ManipulatorRequestHandler::GetStatus()
+    {
         int p;
         CallResult(p, FN_GetStatus);
         return p;
     }
 
-
     void ManipulatorRequestHandler::Reflect(AZ::ReflectContext* context)
     {
         if (AZ::BehaviorContext* behaviorContext = azrtti_cast<AZ::BehaviorContext*>(context))
@@ -45,5 +47,4 @@ namespace AppleKraken
         }
     }
 
-}
-
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/ManipulatorRequestBus.h

@@ -7,12 +7,10 @@
  */
 #pragma once
 
-
 #include <AzCore/EBus/EBus.h>
 #include <AzCore/Math/Vector3.h>
 #include <AzCore/RTTI/BehaviorContext.h>
 
-
 namespace AppleKraken
 {
     class ManipulatorRequest : public AZ::EBusTraits
@@ -25,17 +23,25 @@ namespace AppleKraken
         virtual void PickApple(const AZ::Vector3 position) = 0;
         virtual AZ::Vector3 GetPosition() = 0;
         virtual void Retrieve() = 0;
-        virtual int GetStatus()=0;
+        virtual int GetStatus() = 0;
     };
 
     using ManipulatorRequestBus = AZ::EBus<ManipulatorRequest>;
 
     //! EventHanlder with AZ_EBUS_BEHAVIOR_BINDER to integrate with LUA/Canvas
     class ManipulatorRequestHandler
-            : public ManipulatorRequestBus::Handler, public AZ::BehaviorEBusHandler{
+        : public ManipulatorRequestBus::Handler
+        , public AZ::BehaviorEBusHandler
+    {
     public:
-        AZ_EBUS_BEHAVIOR_BINDER(ManipulatorRequestHandler, "{30CE1753-DEDE-4D83-8C7C-F5F2BBD12DE8}",
-                                AZ::SystemAllocator, PickApple, GetPosition, Retrieve, GetStatus);
+        AZ_EBUS_BEHAVIOR_BINDER(
+            ManipulatorRequestHandler,
+            "{30CE1753-DEDE-4D83-8C7C-F5F2BBD12DE8}",
+            AZ::SystemAllocator,
+            PickApple,
+            GetPosition,
+            Retrieve,
+            GetStatus);
 
         virtual void PickApple(const AZ::Vector3 position) override;
 
@@ -45,6 +51,6 @@ namespace AppleKraken
 
         virtual int GetStatus() override;
 
-        static void Reflect(AZ::ReflectContext *context);
+        static void Reflect(AZ::ReflectContext* context);
     };
-}
+} // namespace AppleKraken

Project/Gem/Source/ROSConDemoSystemComponent.cpp

@@ -20,7 +20,6 @@ namespace ROSConDemo
                     ->Attribute(AZ::Edit::Attributes::AutoExpand, true);
             }
         }
-
     }
 
     void ROSConDemoSystemComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)