Browse Source

Removed lua, moved printf to ImGUI

Signed-off-by: Michał Pełka <[email protected]>
Michał Pełka 2 years ago
parent
commit
5b1ab23e16

+ 0 - 408
Project/Gem/Scripts/apple_kraken_manipulator.lua

@@ -1,408 +0,0 @@
---[[
-    Manipulator control
---]]
-
-function clamp(value, lower_limit, upper_limit)
-    if lower_limit == upper_limit then
-        return value
-    else
-        if value > upper_limit then
-            return upper_limit
-        elseif value < lower_limit then
-            return lower_limit
-        else
-            return value
-        end
-    end
-end
-
-local PID = {}
-PID.__index = PID
-
-function PID.new(kp, kd, ki, limit_output)
-    local self = setmetatable({}, PID)
-    self._kp = kp
-    self._kd = kd
-    self._ki = ki
-    self._preError = 0
-    self._integral = 0
-    self._limit = limit_output
-    return self
-end
-
-function PID:Reset()
-    self._preError = 0
-    self._integral = 0
-end
-
-function PID:Calculate(dt, err)
-    local pOut = (self._kp * err)
-    self._integral = self._integral + (err * dt)
-    local iOut = (self._ki * self._integral)
-    local deriv = ((err - self._preError) / dt)
-    local dOut = (self._kd * deriv)
-    output = (pOut + iOut + dOut)
-    self._preError = err
-    if self._limit>0.0 then
-        output = clamp(output, -self._limit, self._limit)
-    end
-    return output
-end
-
-local manipulator_control = 
-{
-    Properties =
-    {
-    segment1 = { default = EntityId() },
-    segment2 = { default = EntityId() },
-    segment3 = { default = EntityId() },
-    segment4 = { default = EntityId() },
-
-    gripper = { default = EntityId() },
-    rest = { default = EntityId() },
-    debug = { default = EntityId() },
-
-    apple1 = { default = EntityId() },
-    apple2 = { default = EntityId() },
-    }
-}
-
-State = {
-    IDLE = 0,  --!< Idle state / position, suitable for robot moving around the environment.
-    PREPARED = 10, --!< State and position which are ready for picking tasks.
-    PICKING_BASE = 20, --!< The effector is on its way to pick fruit.
-    PICKING_NOSE = 25, --!< The effector is on its way to pick fruit.
-    RETRIEVING_NOSE = 30, --!< The effector is retrieving a fruit to storage position.
-    RETRIEVING_BASE = 35, --!< The effector is retrieving a fruit to storage position.
-    INVALID = -1, --!< Invalid state. Requires an additional context that could help user understand what happened. @see PickingState.
-}
-
-function manipulator_control:OnActivate()
-
-    ------------------------------------
-    -- Configuration parameters BEGIN --
-
-    -- Maximum velocity in X (in vehicle axis), Y (suction tube), 
-    -- Z (vertical) directions
-    self.max_velocity = Vector3(40.0, 30.0, 40.0) 
-
-    self.startupWait = 2.0 --[s]
-
-    -- Wait till FPS reaches this value to start the simulation
-    -- to prevent violent reactions right after the simulation starts
-    self.startapMinFPS = 10.0
-
-    -- Zero threshold - is used to check if manipulator reached the destination.
-    -- If absolute value of error (target_position - current_position)
-    -- is lower than these values, it's assumed that destination was reached.
-    -- It is defined for each segment separatelly.
-    self.nearZeroThresholds = {0.1, 0.1, 0.1, 0.1} -- [m]
-
-    -- When manipulator moves, it performes X and Z movements first (movement of 
-    -- the base), when destination is reached (based on self.nearZeroThresholds), 
-    -- it starts moving the suction tube ("nose"). Before moving the nose it 
-    -- waits self.baseZeroTimeout to stabilize.
-    self.baseZeroTimeout = 0.1 --[s]
-
-    -- If true, [Numpad 1] and [Numpad 2] keys can be used to select between 
-    -- "apple1" and "apple2" entities
-    self.enableKeyboardInput = true
-
-    -- If true, current manipulaor state will be printed
-    self.printState = true
-
-    -- Time to wait in when the parking (aka retrieving) position is reached
-    self.retrieveWait = 0.5 -- [s]
-
-    -- If true, apple will be retreived automatically after reaching. If false,
-    -- Retrieve() function must be called.
-    self.autoRetrieve = false
-
-    -- Configuration parameters END --
-    ------------------------------------
-
-    self.tickBusHandler = TickBus.CreateHandler(self)
-    self.tickBusHandler:Connect()    
-
-    self.inputEventBusHandler = InputEventNotificationBus.Connect(self, InputEventNotificationId("manipulator_keyboard_control"))
-
-    self.pid1 = PID.new(300.0, 0.0, 0.0, self.max_velocity['z']) 
-    self.pid2 = PID.new(200.0, 0.0, 0.0, self.max_velocity['x']) 
-    self.pid3 = PID.new(100.0, 0.0, 0.0, self.max_velocity['y'])
-    self.pid4 = PID.new(100.0, 0.0, 0.0, self.max_velocity['y'])
-    self.gravityThreshold = 0.0
-
-    -- Target translation of each segment. Relative to current position
-    self.internalError = {nil, nil, nil, nil}
-
-    -- Target position (x,y,z) in internal reference
-    self.targetPos = Vector3(0.0, 0.0, 0.0)
-
-    -- Target
-    self.requestWorldPos = Vector3(0.0, 0.0, 0.0)
-    self.restPos = Vector3(0.0, -0.1, 0.0)
-
-    self.pickingState = State.PREPARED
-    self.lastPickingState = nil
-
-    self.baseZeroTime = 0.0
-
-    self.retrieveTime = 0.0
-
-    self.startupWait = 0.1 --[s]
-
-    self.noseZeroTimeout = 0.5
-    self.noseZeroTime = 0.0
-    self.manipulatorRequestBus = ManipulatorRequestBus.Connect(self, self.entityId)       
-end
-
-function manipulator_control:_getFPS()
-    -- TODO Apply some kind of filter to smooth this vale
-    -- TODO Can we do it in a different way? Obtain it form a bus?
-    return 1.0 / self.deltaTime
-end
-
-function manipulator_control:getSegmentPos(entityid)
-    return TransformBus.Event.GetLocalTranslation(entityid)
-end
-
-function manipulator_control:_setSegmentPos(entityid, pid, target_pos, axis1, print_debug)
-
-    local err = target_pos
-    local force = pid:Calculate(self.deltaTime, err)
-
-    local impulse = force * self.deltaTime
-
-    local force_vector = Vector3(0.0, 0.0, impulse)
-
-    local tm = TransformBus.Event.GetWorldTM(entityid)
-
-    force_vector = Transform.TransformVector(tm, force_vector)
-
-    -- If we want to use velocity
-    RigidBodyRequestBus.Event.SetLinearVelocity(entityid, force_vector)   
-    -- If we want to use force impulse
-    --RigidBodyRequestBus.Event.ApplyLinearImpulse(entityid, force_vector)   
-
-    if print_debug then
-        Debug.Log('error: '..string.format("%1.3f",target_pos)..'  impulse: '..string.format("%1.3f",force))
-    end
-end
-
-function manipulator_control:_setPosition()
-    self:_setSegmentPos(self.Properties.segment1, self.pid1, self.internalError[1], 'z', false)
-    self:_setSegmentPos(self.Properties.segment2, self.pid2, self.internalError[2], 'x', false)
-    self:_setSegmentPos(self.Properties.segment3, self.pid3, self.internalError[3], 'y', false)
-    self:_setSegmentPos(self.Properties.segment4, self.pid4, self.internalError[4], 'z', false)
-end
-
-
-function manipulator_control:_getWorldPosition(target_position)
-    local tm = TransformBus.Event.GetWorldTM(self.Properties.gripper)
-    local gripper_pos = Transform.GetTranslation(tm)
-    Transform.Invert(tm)
-
-    local pos = Transform.TransformVector(tm, target_position - gripper_pos)
-
-    TransformBus.Event.SetLocalTranslation(self.Properties.debug, pos)
-
-    return pos
-end
-
-function manipulator_control:_setInternalState()
-    self.internalError[1] = -self.targetPos['z'] + self.gravityThreshold 
-    self.internalError[2] = self.targetPos['x'] 
-    self.internalError[3] = self.targetPos['y'] 
-    self.internalError[4] = self.targetPos['y']
-end
-
-function manipulator_control:_isNearZero(segment)
-    if math.abs(self.internalError[segment]) < self.nearZeroThresholds[segment] then
-        return true
-    else
-        return false
-    end
-end
-
-
-function manipulator_control:_printDebugInfo()
-    if self.lastPickingState ~= self.pickingState then
-        self.lastPickingState = self.pickingState
-
-        if self.printState then
-            if self.pickingState == State.IDLE then
-                txt = 'IDLE'
-
-            elseif self.pickingState == State.PREPARED then
-                txt = 'PREPARED'
-
-            elseif self.pickingState == State.PICKING_BASE then
-                txt = 'PICKING_BASE'
-
-            elseif self.pickingState == State.PICKING_NOSE then
-                txt = 'PICKING_NOSE'
-
-            elseif self.pickingState == State.RETRIEVING_NOSE then
-                txt = 'RETRIEVING_NOSE'
-
-            elseif self.pickingState == State.RETRIEVING_BASE then
-                txt = 'RETRIEVING_BASE'
-
-            elseif self.pickingState == State.INVALID then
-                txt = 'INVALID'
-            end
-
-            Debug.Log("Changed state to: ["..txt.."]")
-        end
-    end
-end
-
-function manipulator_control:_orchestrator()
-
-    ----------------------------------------------------------
-
-    if self.pickingState == State.INVALID then
-        self.requestWorldPos = TransformBus.Event.GetWorldTranslation(self.Properties.rest)
-        self.targetPos = self:_getWorldPosition(self.requestWorldPos)
-    end
-
-    ----------------------------------------------------------
-
-    if self.pickingState == State.RETRIEVING_BASE then
-        self.requestWorldPos = TransformBus.Event.GetWorldTranslation(self.Properties.rest)
-        self.targetPos = self:_getWorldPosition(self.requestWorldPos)
-        self:_setInternalState()
-        if (self:_isNearZero(1) and self:_isNearZero(2) and self:_isNearZero(3) and self:_isNearZero(4)) then
-            self.retrieveTime = self.retrieveTime + self.deltaTime
-            if self.retrieveTime > self.retrieveWait then
-                self.retrieveTime = 0.0
-                self.pickingState = State.PREPARED
-            end
-        end
-    end
-
-    ----------------------------------------------------------
-
-    if self.pickingState == State.RETRIEVING_NOSE then
-
-        self.requestWorldPos = TransformBus.Event.GetWorldTranslation(self.Properties.rest)
-        self.targetPos['y'] = self:_getWorldPosition(self.requestWorldPos)['y']
-
-        if (self:_isNearZero(3) and self:_isNearZero(4)) then
-            self.noseZeroTime = self.noseZeroTime + self.deltaTime
-            if self.noseZeroTime > self.noseZeroTimeout then
-                self.noseZeroTime = 0.0
-                self.pickingState = State.RETRIEVING_BASE
-            end
-        end
-    end
-
-    ----------------------------------------------------------
-
-    if self.pickingState == State.PREPARED then
-        self.requestWorldPos = TransformBus.Event.GetWorldTranslation(self.Properties.rest)
-        self.targetPos = self:_getWorldPosition(self.requestWorldPos)
-    end
-
-    ----------------------------------------------------------
-
-    if self.pickingState == State.PICKING_NOSE then
-
-        self.targetPos = self:_getWorldPosition(self.requestWorldPos)
-
-        if (self:_isNearZero(3) and self:_isNearZero(4)) then
-            self.noseZeroTime = self.noseZeroTime + self.deltaTime
-            if self.noseZeroTime > self.noseZeroTimeout then
-                self.noseZeroTime = 0.0
-                self.pickingTime = 0.0
-                if self.autoRetrieve then
-                    self:Retrieve()
-                end
-            end
-        end
-    end
-
-    ----------------------------------------------------------
-
-    if self.pickingState == State.PICKING_BASE then
-        self.targetPos = self:_getWorldPosition(self.requestWorldPos)
-        self.targetPos['y'] = self.restPos['y']
-
-        if (self:_isNearZero(1) and self:_isNearZero(2)) then
-            self.baseZeroTime = self.baseZeroTime + self.deltaTime
-            if self.baseZeroTime > self.baseZeroTimeout then
-                self.baseZeroTime = 0.0
-                self.pickingState = State.PICKING_NOSE
-            end
-        end
-    end
-
-    ----------------------------------------------------------
-
-    self:_setInternalState()
-    self:_setPosition()
-    self:_printDebugInfo()
-end
-
-function manipulator_control:PickApple(apple_pos)
-    --if self.pickingState == state.PREPARED then
-
-    self.requestWorldPos = apple_pos
-    self.pickingState = State.PICKING_BASE
-
-    Debug.Log("Picking apple at "..tostring(apple_pos))
-
-    --end
-end
-
-
-function manipulator_control:Retrieve()
-    self.pickingState = State.RETRIEVING_NOSE
-    --Debug.Log("Retracting to "..tostring(rest_pos))
-end
-
-function manipulator_control:GetStatus()
-    return self.pickingState
-end
-
-
-
-function manipulator_control:OnPressed(value)
-    -- Keypress actions
-
-    if self.enableKeyboardInput then
-
-        if value == 1.0 then
-            local currentApple = self.Properties.apple1
-            if currentApple ~= nil then
-                self:PickApple(TransformBus.Event.GetWorldTranslation(currentApple))
-            end
-        end
-
-        if value == 2.0 then
-            local currentApple = self.Properties.apple2
-            if currentApple ~= nil then
-                self:PickApple(TransformBus.Event.GetWorldTranslation(currentApple))
-            end
-        end 
-    end
-    
-end
-
-function manipulator_control:OnDeactivate()
-    self.manipulatorRequestBus:Disconnect()
-    self.tickBusHandler:Disconnect()
-    self.inputEventBusHandler:Disconnect()
-end
-
--- This callback is called every frame by the tick bus after this entity activates
-function manipulator_control:OnTick(deltaTime, timePoint)
-
-    self.deltaTime = deltaTime
-
-    if self.Properties.segment1~=nil then
-        self:_orchestrator()
-    end
-end
-
-return manipulator_control

+ 34 - 26
Project/Gem/Source/ApplePicker/KrakenEffectorComponent.cpp

@@ -30,10 +30,12 @@ namespace AppleKraken
                                                                                 { EffectorState::PREPARED, "PREPARED" },
                                                                                 { EffectorState::PREPARED, "PREPARED" },
                                                                                 { EffectorState::PICKING, "PICKING" },
                                                                                 { EffectorState::PICKING, "PICKING" },
                                                                                 { EffectorState::PICKING_STABILIZE, "PICKING_STABILIZE" },
                                                                                 { EffectorState::PICKING_STABILIZE, "PICKING_STABILIZE" },
-                                                                                { EffectorState::RETRIEVING, "RETRIEVING" },
                                                                                 { EffectorState::RETRIEVING_NOSE, "RETRIEVING_NOSE" },
                                                                                 { EffectorState::RETRIEVING_NOSE, "RETRIEVING_NOSE" },
+                                                                                { EffectorState::RETRIEVING, "RETRIEVING" },
                                                                                 { EffectorState::RETRIEVING_STABILIZE,
                                                                                 { EffectorState::RETRIEVING_STABILIZE,
-                                                                                  "RETRIEVING_STABILIZE" } };
+                                                                                  "RETRIEVING_STABILIZE" },
+                                                                                { EffectorState::RETRIEVING_FAILED,
+                                                                                            "RETRIEVING_FAILED" }  };
 
 
         // TODO - this is a debug space for a stub implementation. Proper: a state transition machine with lambdas.
         // TODO - this is a debug space for a stub implementation. Proper: a state transition machine with lambdas.
         AZStd::string StateTransitionString(EffectorState current, EffectorState next)
         AZStd::string StateTransitionString(EffectorState current, EffectorState next)
@@ -147,7 +149,6 @@ namespace AppleKraken
                     ->Attribute(AZ::Edit::Attributes::AutoExpand, true);
                     ->Attribute(AZ::Edit::Attributes::AutoExpand, true);
             }
             }
         }
         }
-        ManipulatorRequestHandler::Reflect(context);
     }
     }
 
 
     void KrakenEffectorComponent::PrepareForPicking()
     void KrakenEffectorComponent::PrepareForPicking()
@@ -186,7 +187,6 @@ namespace AppleKraken
 
 
     AZ::Obb KrakenEffectorComponent::GetEffectorReachArea()
     AZ::Obb KrakenEffectorComponent::GetEffectorReachArea()
     {
     {
-        AZ_TracePrintf("KrakenEffectorComponent", "%s: GetEffectorReachArea\n", GetEntity()->GetName().c_str());
         AZ::Obb reachArea;
         AZ::Obb reachArea;
 
 
         if (m_reachEntity.IsValid())
         if (m_reachEntity.IsValid())
@@ -198,23 +198,6 @@ namespace AppleKraken
                 dimensions, m_reachEntity, &LmbrCentral::BoxShapeComponentRequests::GetBoxDimensions);
                 dimensions, m_reachEntity, &LmbrCentral::BoxShapeComponentRequests::GetBoxDimensions);
             if (!dimensions.IsZero())
             if (!dimensions.IsZero())
             {
             {
-                AZ_Printf("KrakenEffectorComponent", "OurEffectorReachArea :");
-                AZ_Printf(
-                    "KrakenEffectorComponent", "  local dimensions : %f %f %f", dimensions.GetX(), dimensions.GetY(), dimensions.GetZ());
-                AZ_Printf(
-                    "KrakenEffectorComponent",
-                    "  transform - rot  : %f %f %f %f",
-                    targetTM.GetRotation().GetX(),
-                    targetTM.GetRotation().GetY(),
-                    targetTM.GetRotation().GetZ(),
-                    targetTM.GetRotation().GetW());
-                AZ_Printf(
-                    "KrakenEffectorComponent",
-                    "  transform - pos  : %f %f %f",
-                    targetTM.GetTranslation().GetX(),
-                    targetTM.GetTranslation().GetY(),
-                    targetTM.GetTranslation().GetZ());
-
                 reachArea.SetHalfLengths(dimensions / 2);
                 reachArea.SetHalfLengths(dimensions / 2);
                 reachArea.SetPosition(targetTM.GetTranslation());
                 reachArea.SetPosition(targetTM.GetTranslation());
                 reachArea.SetRotation(targetTM.GetRotation());
                 reachArea.SetRotation(targetTM.GetRotation());
@@ -522,12 +505,37 @@ namespace AppleKraken
 
 
     void KrakenEffectorComponent::OnImGuiUpdate(){
     void KrakenEffectorComponent::OnImGuiUpdate(){
 
 
-        AZStd::string window_name("ManipulatorController %s", GetEntityId().ToString().c_str()));
+        AZStd::string window_name = AZStd::string::format("ManipulatorController%s", GetEntityId().ToString().c_str());
         ImGui::Begin(window_name.c_str());
         ImGui::Begin(window_name.c_str());
-        if (ImGui::CollapsingHeader("KrakenEffectorComponent"))
-        {
-            const auto & state_name = DebugStateTransit::kMapToString.at(m_effectorState);
-            ImGui::Text("state : %s",state_name);
+        const auto & state_name = DebugStateTransit::kMapToString.at(m_effectorState);
+        ImGui::Text("m_effectorState : %s",state_name);
+        ImGui::BeginGroup();
+        ImGui::Text("m_currentTask:");
+        ImGui::Text("m_appleEntityId : %s",m_currentTask.m_appleEntityId.ToString().c_str());
+        ImGui::Text("m_middle : %.1f %.1f %.1f",m_currentTask.m_middle.GetX(),m_currentTask.m_middle.GetY(),m_currentTask.m_middle.GetZ());
+        ImGui::EndGroup();
+        if (ImGui::CollapsingHeader("KrakenTestApplePicking") && m_reachEntity.IsValid()) {
+            AZ::Obb r = KrakenEffectorComponent::GetEffectorReachArea();
+
+            ImGui::SliderFloat("Horizontal", &m_debugApple[0], -r.GetHalfLengthX(), r.GetHalfLengthX());
+            ImGui::SliderFloat("Vertical", &m_debugApple[2], -r.GetHalfLengthZ(), r.GetHalfLengthZ());
+            ImGui::SliderFloat("Nose", &m_debugApple[1], -r.GetHalfLengthY(), r.GetHalfLengthY());
+
+            if (ImGui::Button("Send 'PickApple'")) {
+                AZ::Transform targetTM = AZ::Transform::CreateIdentity();
+                AZ::TransformBus::EventResult(targetTM, m_reachEntity, &AZ::TransformBus::Events::GetWorldTM);
+                PickAppleTask appleTask;
+                appleTask.m_middle = targetTM.TransformPoint(AZ::Vector3::CreateFromFloat3(m_debugApple.data()));
+                PickApple(appleTask);
+            }
+            ImGui::SameLine();
+            if (ImGui::Button("Send 'PrepareForPicking'")) {
+                PrepareForPicking();
+            }
+            ImGui::SameLine();
+            if (ImGui::Button("Send 'FinishPicking'")) {
+                FinishPicking();
+            }
         }
         }
         ImGui::End();
         ImGui::End();
     };
     };

+ 1 - 0
Project/Gem/Source/ApplePicker/KrakenEffectorComponent.h

@@ -87,5 +87,6 @@ namespace AppleKraken
         bool m_registeredCallback{ false };
         bool m_registeredCallback{ false };
         bool is_manipulator_locked = { false };
         bool is_manipulator_locked = { false };
         AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler m_onTriggerHandleBeginHandler;
         AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler m_onTriggerHandleBeginHandler;
+        std::array<float,3> m_debugApple;
     };
     };
 } // namespace AppleKraken
 } // namespace AppleKraken

+ 0 - 1
Project/Gem/Source/ApplePicker/PickingStructs.h

@@ -24,7 +24,6 @@ namespace AppleKraken
         RETRIEVING = 35, //!< The effector is retrieving a fruit to storage position.
         RETRIEVING = 35, //!< The effector is retrieving a fruit to storage position.
         RETRIEVING_STABILIZE = 40, //!< The effector is retrieving, wait for apple to drop
         RETRIEVING_STABILIZE = 40, //!< The effector is retrieving, wait for apple to drop
         RETRIEVING_FAILED = 50 //!< The effector is retrieving after failing to pick the apple.
         RETRIEVING_FAILED = 50 //!< The effector is retrieving after failing to pick the apple.
- 
     };
     };
 
 
     //! A task to pick a single apple.
     //! A task to pick a single apple.

+ 9 - 8
Project/Gem/Source/Manipulator/KrakenManipulatorController.cpp

@@ -132,7 +132,7 @@ namespace AppleKraken
             }
             }
         }
         }
         // auto - disable nose retrieve only if we reached small error.
         // auto - disable nose retrieve only if we reached small error.
-        if (m_noseRetrieved == true)
+        if (m_noseRetrieveRequest == true)
         {
         {
             m_time_XZ_ok += deltaTime;
             m_time_XZ_ok += deltaTime;
             if (m_time_XZ_ok > m_timeSetpointReach)
             if (m_time_XZ_ok > m_timeSetpointReach)
@@ -140,7 +140,7 @@ namespace AppleKraken
                 if (error_x < max_errorXZ  && error_x > -max_errorXZ && error_z < max_errorXZ && error_z > -max_errorXZ)
                 if (error_x < max_errorXZ  && error_x > -max_errorXZ && error_z < max_errorXZ && error_z > -max_errorXZ)
                 {
                 {
                     AZ_Printf("ManipulatorController", "Nose is sliding out  \n");
                     AZ_Printf("ManipulatorController", "Nose is sliding out  \n");
-                    m_noseRetrieved = false;
+                    m_noseRetrieveRequest = false;
                     m_time_XZ_ok = 0;
                     m_time_XZ_ok = 0;
                 }
                 }
             }
             }
@@ -151,7 +151,7 @@ namespace AppleKraken
             auto component_y = getMotorizedJoint(m_entityY);
             auto component_y = getMotorizedJoint(m_entityY);
             if (component_y)
             if (component_y)
             {
             {
-                if (m_noseRetrieved)
+                if (m_noseRetrieveRequest)
                 {
                 {
                     m_setPointY = 0;
                     m_setPointY = 0;
                     m_time_Y_ok += deltaTime;
                     m_time_Y_ok += deltaTime;
@@ -178,7 +178,7 @@ namespace AppleKraken
 
 
     void ManipulatorController::PickApple(const AZ::Vector3 position)
     void ManipulatorController::PickApple(const AZ::Vector3 position)
     {
     {
-        m_noseRetrieved = true;
+        m_noseRetrieveRequest = true;
         m_time_XZ_ok = 0;
         m_time_XZ_ok = 0;
         AZ_Printf("ManipulatorController", "PickApple\n");
         AZ_Printf("ManipulatorController", "PickApple\n");
         ResetTimer();
         ResetTimer();
@@ -199,14 +199,14 @@ namespace AppleKraken
     {
     {
         AZ_Printf("ManipulatorController", "Retrieve\n");
         AZ_Printf("ManipulatorController", "Retrieve\n");
         m_time_XZ_ok = std::numeric_limits<float>::lowest();
         m_time_XZ_ok = std::numeric_limits<float>::lowest();
-        m_noseRetrieved = true;
+        m_noseRetrieveRequest = true;
         m_desiredApple.reset();
         m_desiredApple.reset();
     };
     };
     void ManipulatorController::RetrieveNose()
     void ManipulatorController::RetrieveNose()
     {
     {
         AZ_Printf("ManipulatorController", "RetrieveNose\n");
         AZ_Printf("ManipulatorController", "RetrieveNose\n");
         m_time_XZ_ok = std::numeric_limits<float>::lowest();
         m_time_XZ_ok = std::numeric_limits<float>::lowest();
-        m_noseRetrieved = true;
+        m_noseRetrieveRequest = true;
     }
     }
 
 
     int ManipulatorController::GetStatus()
     int ManipulatorController::GetStatus()
@@ -216,7 +216,6 @@ namespace AppleKraken
 
 
     void ManipulatorController::ResetTimer()
     void ManipulatorController::ResetTimer()
     {
     {
-        AZ_Printf("ManipulatorController", "Timer is RESET!\n");
         m_time_XZ_ok = 0;
         m_time_XZ_ok = 0;
     }
     }
 
 
@@ -238,7 +237,7 @@ namespace AppleKraken
     void ManipulatorController::OnImGuiUpdate()
     void ManipulatorController::OnImGuiUpdate()
     {
     {
 
 
-        AZStd::string window_name("ManipulatorController %s", GetEntityId().ToString().c_str()));
+        AZStd::string window_name = AZStd::string::format("ManipulatorController%s", GetEntityId().ToString().c_str());
         ImGui::Begin(window_name.c_str());
         ImGui::Begin(window_name.c_str());
         auto pos = GetPosition();
         auto pos = GetPosition();
         if (m_desiredApple){
         if (m_desiredApple){
@@ -255,6 +254,8 @@ namespace AppleKraken
         {
         {
             ImGui::Text("Positions : %.1f", pos.GetY());
             ImGui::Text("Positions : %.1f", pos.GetY());
             ImGui::Text("SetPoint  : %.1f", m_setPointY);
             ImGui::Text("SetPoint  : %.1f", m_setPointY);
+            ImGui::Checkbox("noseRetrieveRequest", &m_noseRetrieveRequest);
+            ImGui::Checkbox("noseRetrievingSuccess", &m_noseRetrievingSuccess);
         }
         }
         ImGui::End();
         ImGui::End();
 
 

+ 1 - 1
Project/Gem/Source/Manipulator/KrakenManipulatorController.h

@@ -53,7 +53,7 @@ namespace AppleKraken
 
 
         AZ::Vector3 m_desiredPosition{0, 0, 0 };
         AZ::Vector3 m_desiredPosition{0, 0, 0 };
         AZStd::optional<AZ::Vector3> m_desiredApple;
         AZStd::optional<AZ::Vector3> m_desiredApple;
-        bool m_noseRetrieved{false };
+        bool m_noseRetrieveRequest{false };
         bool m_noseRetrievingSuccess{false};
         bool m_noseRetrievingSuccess{false};
 
 
         AZ::Vector3 m_vectorX{1, 0, 0 };
         AZ::Vector3 m_vectorX{1, 0, 0 };

+ 0 - 77
Project/Gem/Source/Manipulator/ManipulatorRequestBus.cpp

@@ -1,77 +0,0 @@
-
-/*
- * Copyright (c) Contributors to the Open 3D Engine Project.
- * For complete copyright and license terms please see the LICENSE at the root of this distribution.
- *
- * SPDX-License-Identifier: Apache-2.0 OR MIT
- *
- */
-#include "ManipulatorRequestBus.h"
-
-namespace AppleKraken
-{
-    void ManipulatorRequestHandler::PickApple(const AZ::Vector3 position)
-    {
-        Call(FN_PickApple, position);
-    }
-
-    AZ::Vector3 ManipulatorRequestHandler::GetPosition()
-    {
-        AZ::Vector3 p;
-        CallResult(p, FN_GetPosition);
-        return p;
-    }
-
-    void ManipulatorRequestHandler::Retrieve()
-    {
-        Call(FN_Retrieve);
-    }
-
-    void ManipulatorRequestHandler::RetrieveNose()
-    {
-        Call(FN_RetrieveNose);
-    }
-    int ManipulatorRequestHandler::GetStatus()
-    {
-        int p;
-        CallResult(p, FN_GetStatus);
-        return p;
-    }
-    bool ManipulatorRequestHandler::IsNoseRetreived()
-    {
-        bool p;
-        CallResult(p, FN_IsNoseRetreived);
-        return p;
-    }
-    AZ::EntityId ManipulatorRequestHandler::GetEffectorEntity()
-    {
-        AZ::EntityId p;
-        CallResult(p, FN_GetEffectorEntity);
-        return p;
-    }
-    AZ::EntityId ManipulatorRequestHandler::GetRestEntity()
-    {
-        AZ::EntityId p;
-        CallResult(p, FN_GetRestEntity);
-        return p;
-    }
-
-
-    void ManipulatorRequestHandler::Reflect(AZ::ReflectContext* context)
-    {
-        if (AZ::BehaviorContext* behaviorContext = azrtti_cast<AZ::BehaviorContext*>(context))
-        {
-            behaviorContext->EBus<ManipulatorRequestBus>("ManipulatorRequestBus")
-                ->Handler<ManipulatorRequestHandler>()
-                ->Event("PickApple", &ManipulatorRequestBus::Events::PickApple)
-                ->Event("GetPosition", &ManipulatorRequestBus::Events::GetPosition)
-                ->Event("Retrieve", &ManipulatorRequestBus::Events::Retrieve)
-                ->Event("GetStatus", &ManipulatorRequestBus::Events::GetStatus)
-                ->Event("GetEffectorEntity", &ManipulatorRequestBus::Events::GetEffectorEntity)
-                ->Event("GetRestEntity", &ManipulatorRequestBus::Events::GetRestEntity)
-                ->Event("IsNoseRetreived", &ManipulatorRequestBus::Events::IsNoseRetreived)
-                ->Event("RetrieveNose", &ManipulatorRequestBus::Events::RetrieveNose);
-        }
-    }
-
-} // namespace AppleKraken

+ 0 - 37
Project/Gem/Source/Manipulator/ManipulatorRequestBus.h

@@ -32,41 +32,4 @@ namespace AppleKraken
 
 
     using ManipulatorRequestBus = AZ::EBus<ManipulatorRequest>;
     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:
-        AZ_EBUS_BEHAVIOR_BINDER(
-            ManipulatorRequestHandler,
-            "{30CE1753-DEDE-4D83-8C7C-F5F2BBD12DE8}",
-            AZ::SystemAllocator,
-            PickApple,
-            GetPosition,
-            Retrieve,
-            RetrieveNose,
-            GetStatus,
-            IsNoseRetreived,
-            GetEffectorEntity,
-            GetRestEntity);
-
-        virtual void PickApple(const AZ::Vector3 position) override;
-
-        virtual AZ::Vector3 GetPosition() override;
-
-        virtual void Retrieve() override;
-
-        virtual void RetrieveNose() override;
-
-        virtual int GetStatus() override;
-
-        virtual bool IsNoseRetreived() override;
-
-        virtual AZ::EntityId GetEffectorEntity() override;
-
-        virtual AZ::EntityId GetRestEntity() override;
-
-        static void Reflect(AZ::ReflectContext* context);
-    };
 } // namespace AppleKraken
 } // namespace AppleKraken