Move Joint Tracjectory State to TrajectoryComponent (#808)

Signed-off-by: Mateusz Wasilewski <[email protected]>

Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.cpp

@@ -23,16 +23,6 @@ namespace ROS2
             AZStd::bind(&FollowJointTrajectoryActionServer::GoalAcceptedCallback, this, AZStd::placeholders::_1));
     }
 
-    JointsTrajectoryRequests::TrajectoryActionStatus FollowJointTrajectoryActionServer::GetGoalStatus() const
-    {
-        return m_goalStatus;
-    }
-
-    void FollowJointTrajectoryActionServer::SetGoalSuccess()
-    {
-        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Succeeded;
-    }
-
     void FollowJointTrajectoryActionServer::CancelGoal(std::shared_ptr<FollowJointTrajectory::Result> result)
     {
         AZ_Assert(m_goalHandle, "Invalid goal handle!");
@@ -50,7 +40,6 @@ namespace ROS2
         {
             AZ_Trace("FollowJointTrajectoryActionServer", "Goal succeeded\n");
             m_goalHandle->succeed(result);
-            m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Succeeded;
         }
     }
 
@@ -102,7 +91,6 @@ namespace ROS2
             return rclcpp_action::CancelResponse::REJECT;
         }
 
-        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Cancelled;
         return rclcpp_action::CancelResponse::ACCEPT;
     }
 
@@ -149,6 +137,5 @@ namespace ROS2
 
         m_goalHandle = goalHandle;
         // m_goalHandle->execute(); // No need to call this, as we are already executing the goal due to ACCEPT_AND_EXECUTE
-        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Executing;
     }
 } // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.h

@@ -32,17 +32,10 @@ namespace ROS2
         //! server documentation </a>
         FollowJointTrajectoryActionServer(const AZStd::string& actionName, const AZ::EntityId& entityId);
 
-        //! Return trajectory action status.
-        //! @return Status of the trajectory execution.
-        JointsTrajectoryRequests::TrajectoryActionStatus GetGoalStatus() const;
-
         //! Cancel the current goal.
         //! @param result Result to be passed to through action server to the client.
         void CancelGoal(std::shared_ptr<FollowJointTrajectory::Result> result);
 
-        //! Sets the goal status to success
-        void SetGoalSuccess();
-
         //! Report goal success to the action server.
         //! @param result Result which contains success code.
         void GoalSuccess(std::shared_ptr<FollowJointTrajectory::Result> result);
@@ -56,7 +49,6 @@ namespace ROS2
         using TrajectoryActionStatus = JointsTrajectoryRequests::TrajectoryActionStatus;
 
         AZ::EntityId m_entityId;
-        TrajectoryActionStatus m_goalStatus = TrajectoryActionStatus::Idle;
         rclcpp_action::Server<FollowJointTrajectory>::SharedPtr m_actionServer;
         std::shared_ptr<GoalHandle> m_goalHandle;
 

Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp

@@ -12,8 +12,8 @@
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/Manipulation/JointsManipulationRequests.h>
 #include <ROS2/ROS2Bus.h>
-#include <ROS2/Utilities/ROS2Names.h>
 #include <ROS2/Utilities/ROS2Conversions.h>
+#include <ROS2/Utilities/ROS2Names.h>
 
 namespace ROS2
 {
@@ -92,7 +92,7 @@ namespace ROS2
     AZ::Outcome<void, JointsTrajectoryComponent::TrajectoryResult> JointsTrajectoryComponent::StartTrajectoryGoal(
         TrajectoryGoalPtr trajectoryGoal)
     {
-        if (m_trajectoryInProgress)
+        if (m_goalStatus == JointsTrajectoryRequests::TrajectoryActionStatus::Executing)
         {
             auto result = JointsTrajectoryComponent::TrajectoryResult();
             result.error_code = JointsTrajectoryComponent::TrajectoryResult::INVALID_GOAL;
@@ -107,7 +107,7 @@ namespace ROS2
         }
         m_trajectoryGoal = *trajectoryGoal;
         m_trajectoryExecutionStartTime = rclcpp::Time(ROS2::ROS2Interface::Get()->GetROSTimestamp());
-        m_trajectoryInProgress = true;
+        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Executing;
         return AZ::Success();
     }
 
@@ -186,13 +186,13 @@ namespace ROS2
     AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::CancelTrajectoryGoal()
     {
         m_trajectoryGoal.trajectory.points.clear();
-        m_trajectoryInProgress = false;
+        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Cancelled;
         return AZ::Success();
     }
 
     JointsTrajectoryRequests::TrajectoryActionStatus JointsTrajectoryComponent::GetGoalStatus()
     {
-        return m_followTrajectoryServer->GetGoalStatus();
+        return m_goalStatus;
     }
 
     void JointsTrajectoryComponent::FollowTrajectory(const uint64_t deltaTimeNs)
@@ -205,7 +205,6 @@ namespace ROS2
             result->error_string = "User Cancelled";
             result->error_code = FollowJointTrajectoryActionServer::FollowJointTrajectory::Result::SUCCESSFUL;
             m_followTrajectoryServer->CancelGoal(result);
-            m_followTrajectoryServer->SetGoalSuccess();
             return;
         }
 
@@ -219,7 +218,7 @@ namespace ROS2
             AZ_TracePrintf("JointsManipulationComponent", "Goal Concluded: all points reached\n");
             auto successResult = std::make_shared<control_msgs::action::FollowJointTrajectory::Result>(); //!< Empty defaults to success.
             m_followTrajectoryServer->GoalSuccess(successResult);
-            m_trajectoryInProgress = false;
+            m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Succeeded;
             return;
         }
 

Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h

@@ -64,9 +64,9 @@ namespace ROS2
         AZStd::string m_followTrajectoryActionName{ "arm_controller/follow_joint_trajectory" };
         AZStd::unique_ptr<FollowJointTrajectoryActionServer> m_followTrajectoryServer;
         TrajectoryGoal m_trajectoryGoal;
+        TrajectoryActionStatus m_goalStatus = TrajectoryActionStatus::Idle;
         rclcpp::Time m_trajectoryExecutionStartTime;
         ManipulationJoints m_manipulationJoints;
-        bool m_trajectoryInProgress{ false };
         builtin_interfaces::msg::Time m_lastTickTimestamp; //!< ROS 2 Timestamp during last OnTick call
     };
 } // namespace ROS2