Fix crash after moveit action stop

Signed-off-by: Artur Kamieniecki <[email protected]>
o3de · Aug 2, 2023 · e334b09 · e334b09
1 parent 91495fc
commit e334b09
Show file tree

Hide file tree

Showing 6 changed files with 21 additions and 11 deletions.
diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsTrajectoryRequests.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsTrajectoryRequests.h
@@ -45,7 +45,7 @@ namespace ROS2
         //! Cancel current trajectory goal.
         //! @param result Result of trajectory goal with explanation on why it was cancelled.
         //! @return nothing on success, error if the goal could not be cancelled.
-        virtual AZ::Outcome<void, AZStd::string> CancelTrajectoryGoal(TrajectoryResultPtr result) = 0;
+        virtual AZ::Outcome<void, AZStd::string> CancelTrajectoryGoal() = 0;
 
         //! Retrieve current trajectory goal status.
         //! @return Status of trajectory goal.

diff --git a/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.cpp b/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.cpp
@@ -28,10 +28,15 @@ namespace ROS2
         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!");
-        if (m_goalHandle && m_goalHandle->is_executing())
+        if (m_goalHandle && m_goalHandle->is_canceling())
         {
             AZ_Trace("FollowJointTrajectoryActionServer", "Cancelling goal\n");
             m_goalHandle->canceled(result);
@@ -88,12 +93,8 @@ namespace ROS2
     rclcpp_action::CancelResponse FollowJointTrajectoryActionServer::GoalCancelledCallback(
         [[maybe_unused]] const std::shared_ptr<GoalHandle> goalHandle)
     { // Accept each cancel attempt
-        auto result = std::make_shared<FollowJointTrajectory::Result>();
-        result->error_string = "User Cancelled";
-        result->error_code = FollowJointTrajectory::Result::SUCCESSFUL;
-
         AZ::Outcome<void, AZStd::string> cancelOutcome;
-        JointsTrajectoryRequestBus::EventResult(cancelOutcome, m_entityId, &JointsTrajectoryRequests::CancelTrajectoryGoal, result);
+        JointsTrajectoryRequestBus::EventResult(cancelOutcome, m_entityId, &JointsTrajectoryRequests::CancelTrajectoryGoal);
 
         if (!cancelOutcome)
         { // This will not happen in simulation unless intentionally done for behavior validation

diff --git a/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.h b/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.h
@@ -40,6 +40,9 @@ namespace ROS2
         //! @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);

diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp
@@ -12,6 +12,7 @@
 #include "JointStatePublisher.h"
 #include <AzCore/Component/ComponentApplicationBus.h>
 #include <AzCore/Component/TransformBus.h>
+#include <AzCore/Debug/Trace.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h>
@@ -443,7 +444,8 @@ namespace ROS2
     void JointsManipulationComponent::Stop()
     {
         for (auto& [jointName, jointInfo] : m_manipulationJoints)
-        { // Set all target joint positions to their current positions.
+        { // Set all target joint positions to their current positions. There is no need to check if the outcome is successful, because
+          // jointName is always valid.
             jointInfo.m_restPosition = GetJointPosition(jointName).GetValue();
         }
     }

diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp
@@ -174,10 +174,9 @@ namespace ROS2
         m_followTrajectoryServer->PublishFeedback(feedback);
     }
 
-    AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::CancelTrajectoryGoal(TrajectoryResultPtr result)
+    AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::CancelTrajectoryGoal()
     {
         m_trajectoryGoal.trajectory.points.clear();
-        m_followTrajectoryServer->CancelGoal(result);
         m_trajectoryInProgress = false;
         return AZ::Success();
     }
@@ -193,6 +192,11 @@ namespace ROS2
         if (goalStatus == JointsTrajectoryRequests::TrajectoryActionStatus::Cancelled)
         {
             JointsManipulationRequestBus::Event(GetEntityId(), &JointsManipulationRequests::Stop);
+            auto result = std::make_shared<FollowJointTrajectoryActionServer::FollowJointTrajectory::Result>();
+            result->error_string = "User Cancelled";
+            result->error_code = FollowJointTrajectoryActionServer::FollowJointTrajectory::Result::SUCCESSFUL;
+            m_followTrajectoryServer->CancelGoal(result);
+            m_followTrajectoryServer->SetGoalSuccess();
             return;
         }
 

diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h
@@ -38,7 +38,7 @@ namespace ROS2
         //! @see ROS2::JointsTrajectoryRequestBus::StartTrajectoryGoal
         AZ::Outcome<void, TrajectoryResult> StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) override;
         //! @see ROS2::JointsTrajectoryRequestBus::CancelTrajectoryGoal
-        AZ::Outcome<void, AZStd::string> CancelTrajectoryGoal(TrajectoryResultPtr trajectoryResult) override;
+        AZ::Outcome<void, AZStd::string> CancelTrajectoryGoal() override;
         //! @see ROS2::JointsTrajectoryRequestBus::GetGoalStatus
         TrajectoryActionStatus GetGoalStatus() override;