Merge pull request #5 from nobleo/fix/HR2-239/robot-stops-too-fast-af…

…ter-cancelling

Fix bug that uses the status of a previous goal

smach_ros/smach_ros/simple_action_state.py

@@ -130,7 +130,7 @@ def __init__(self,
         self._action_spec = action_spec
 
         # Set timeouts
-        self._goal_status = 0
+        self._goal_status = GoalStatus.STATUS_UNKNOWN
         self._goal_result = None
         self._exec_timeout = exec_timeout
         self._preempt_timeout = preempt_timeout
@@ -354,6 +354,7 @@ def execute(self, ud):
         # Activate the state before sending the goal
         self._activate_time = self.node.get_clock().now()
         self._status = ActionState.ACTIVE
+        self._goal_status = GoalStatus.STATUS_UNKNOWN
 
         with self._done_cond:
             # Dispatch goal via non-blocking call to action server
@@ -440,6 +441,7 @@ def _goal_accepted_cb(self, future):
         Accept or reject a client request to begin an action.
         """
         goal_handle = future.result()
+        self._goal_status = goal_handle.status
 
         if not goal_handle.accepted:
             self.node.get_logger().debug(f"Goal request from action {self._action_name} has been rejected!")