Error messages in Joint Trajectory

Gems/ROS2/Code/Include/ROS2/Manipulation/JointsTrajectoryRequests.h

@@ -24,6 +24,7 @@ namespace ROS2
 
         using TrajectoryGoal = control_msgs::action::FollowJointTrajectory::Goal;
         using TrajectoryGoalPtr = std::shared_ptr<const TrajectoryGoal>;
+        using TrajectoryResult = control_msgs::action::FollowJointTrajectory::Result;
         using TrajectoryResultPtr = std::shared_ptr<control_msgs::action::FollowJointTrajectory::Result>;
 
         //! Status of trajectory action.
@@ -39,7 +40,7 @@ namespace ROS2
         //! @param trajectoryGoal Specified trajectory including points with timing and tolerances.
         //! @return Nothing on success, error message if failed.
         //! @note The call will return an error if the goal trajectory mismatches joints system.
-        virtual AZ::Outcome<void, AZStd::string> StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) = 0;
+        virtual AZ::Outcome<void, TrajectoryResult> StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) = 0;
 
         //! Cancel current trajectory goal.
         //! @param result Result of trajectory goal with explanation on why it was cancelled.

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

@@ -80,32 +80,8 @@ namespace ROS2
     }
 
     rclcpp_action::GoalResponse FollowJointTrajectoryActionServer::GoalReceivedCallback(
-        [[maybe_unused]] const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const FollowJointTrajectory::Goal> goal)
-    { // Accept each received goal unless other goal is active (no deferring/queuing)
-        if (!IsReadyForExecution())
-        {
-            AZ_Trace("FollowJointTrajectoryActionServer", "Goal rejected: server is not ready for execution!");
-            if (m_goalHandle)
-            {
-                AZ_Trace(
-                    "FollowJointTrajectoryActionServer",
-                    " is_active: %d,  is_executing %d, is_canceling : %d",
-                    m_goalHandle->is_active(),
-                    m_goalHandle->is_executing(),
-                    m_goalHandle->is_canceling());
-            }
-            return rclcpp_action::GoalResponse::REJECT;
-        }
-
-        AZ::Outcome<void, AZStd::string> executionOrderOutcome;
-        JointsTrajectoryRequestBus::EventResult(executionOrderOutcome, m_entityId, &JointsTrajectoryRequests::StartTrajectoryGoal, goal);
-
-        if (!executionOrderOutcome)
-        {
-            AZ_Trace("FollowJointTrajectoryActionServer", "Execution not be accepted: %s", executionOrderOutcome.GetError().c_str());
-            return rclcpp_action::GoalResponse::REJECT;
-        }
-
+        [[maybe_unused]] const rclcpp_action::GoalUUID& uuid, [[maybe_unused]] std::shared_ptr<const FollowJointTrajectory::Goal> goal)
+    { // Accept each received goal. It will be aborted if other goal is active (no deferring/queuing).
         return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
     }
 
@@ -132,6 +108,44 @@ namespace ROS2
     void FollowJointTrajectoryActionServer::GoalAcceptedCallback(const std::shared_ptr<GoalHandle> goalHandle)
     {
         AZ_Trace("FollowJointTrajectoryActionServer", "Goal accepted\n");
+
+        if (!IsReadyForExecution())
+        {
+            AZ_Trace("FollowJointTrajectoryActionServer", "Goal aborted: server is not ready for execution!");
+            if (m_goalHandle)
+            {
+                AZ_Trace(
+                    "FollowJointTrajectoryActionServer",
+                    " is_active: %d,  is_executing %d, is_canceling : %d",
+                    m_goalHandle->is_active(),
+                    m_goalHandle->is_executing(),
+                    m_goalHandle->is_canceling());
+            }
+
+            auto result = std::make_shared<FollowJointTrajectory::Result>();
+            result->error_string = "Goal aborted: server is not ready for execution!";
+            result->error_code = FollowJointTrajectory::Result::INVALID_GOAL;
+            goalHandle->abort(result);
+            return;
+        }
+
+        AZ::Outcome<void, FollowJointTrajectory::Result> executionOrderOutcome;
+        JointsTrajectoryRequestBus::EventResult(
+            executionOrderOutcome, m_entityId, &JointsTrajectoryRequests::StartTrajectoryGoal, goalHandle->get_goal());
+
+        if (!executionOrderOutcome)
+        {
+            AZ_Trace(
+                "FollowJointTrajectoryActionServer",
+                "Execution not be accepted: %s",
+                executionOrderOutcome.GetError().error_string.c_str());
+
+            auto result = std::make_shared<FollowJointTrajectory::Result>(executionOrderOutcome.GetError());
+
+            goalHandle->abort(result);
+            return;
+        }
+
         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;

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

@@ -79,11 +79,15 @@ namespace ROS2
         incompatible.push_back(AZ_CRC_CE("ManipulatorJointTrajectoryService"));
     }
 
-    AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal)
+    AZ::Outcome<void, JointsTrajectoryComponent::TrajectoryResult> JointsTrajectoryComponent::StartTrajectoryGoal(
+        TrajectoryGoalPtr trajectoryGoal)
     {
         if (m_trajectoryInProgress)
         {
-            return AZ::Failure("Another trajectory goal is executing. Wait for completion or cancel it");
+            auto result = JointsTrajectoryComponent::TrajectoryResult();
+            result.error_code = JointsTrajectoryComponent::TrajectoryResult::INVALID_GOAL;
+            result.error_string = "Another trajectory goal is executing. Wait for completion or cancel it";
+            return AZ::Failure(result);
         }
 
         auto validationResult = ValidateGoal(trajectoryGoal);
@@ -97,19 +101,22 @@ namespace ROS2
         return AZ::Success();
     }
 
-    AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::ValidateGoal(TrajectoryGoalPtr trajectoryGoal)
+    AZ::Outcome<void, JointsTrajectoryComponent::TrajectoryResult> JointsTrajectoryComponent::ValidateGoal(TrajectoryGoalPtr trajectoryGoal)
     {
         // Check joint names validity
         for (const auto& jointName : trajectoryGoal->trajectory.joint_names)
         {
             AZStd::string azJointName(jointName.c_str());
             if (m_manipulationJoints.find(azJointName) == m_manipulationJoints.end())
             {
-                AZ_Printf(
-                    "JointsTrajectoryComponent",
-                    "Trajectory goal is invalid: no joint %s in manipulator",
-                    azJointName.c_str());
-                return AZ::Failure(AZStd::string::format("Trajectory goal is invalid: no joint %s in manipulator", azJointName.c_str()));
+                AZ_Printf("JointsTrajectoryComponent", "Trajectory goal is invalid: no joint %s in manipulator", azJointName.c_str());
+
+                auto result = JointsTrajectoryComponent::TrajectoryResult();
+                result.error_code = JointsTrajectoryComponent::TrajectoryResult::INVALID_JOINTS;
+                result.error_string = std::string(
+                    AZStd::string::format("Trajectory goal is invalid: no joint %s in manipulator", azJointName.c_str()).c_str());
+
+                return AZ::Failure(result);
             }
         }
         return AZ::Success();

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

@@ -36,7 +36,7 @@ namespace ROS2
 
         // JointsTrajectoryRequestBus::Handler overrides ...
         //! @see ROS2::JointsTrajectoryRequestBus::StartTrajectoryGoal
-        AZ::Outcome<void, AZStd::string> StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) override;
+        AZ::Outcome<void, TrajectoryResult> StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) override;
         //! @see ROS2::JointsTrajectoryRequestBus::CancelTrajectoryGoal
         AZ::Outcome<void, AZStd::string> CancelTrajectoryGoal(TrajectoryResultPtr trajectoryResult) override;
         //! @see ROS2::JointsTrajectoryRequestBus::GetGoalStatus
@@ -53,7 +53,7 @@ namespace ROS2
         //! Follow set trajectory.
         //! @param deltaTimeNs frame time step, to advance trajectory by.
         void FollowTrajectory(const uint64_t deltaTimeNs);
-        AZ::Outcome<void, AZStd::string> ValidateGoal(TrajectoryGoalPtr trajectoryGoal);
+        AZ::Outcome<void, TrajectoryResult> ValidateGoal(TrajectoryGoalPtr trajectoryGoal);
         void MoveToNextPoint(const trajectory_msgs::msg::JointTrajectoryPoint currentTrajectoryPoint);
         void UpdateFeedback();